phail
/
bewegungsappar.at_control
10
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Nunchuk running

master
phail 2019-12-24 00:38:06 +01:00
parent 4f53fc3d7b
commit 5ded7076d6
9 changed files with 132 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/config.h
View File

@ -57,6 +57,7 @@ extern comm_settings communicationSettings;
//# define DEBUG_DISABLE_PWMOUTPUT
# define INPUT_NUNCHUK
# define NUNCHUK_VCCPIN 12
# define NUNCHUK_SDAPIN 15
# define NUNCHUK_SCLPIN 4
# define DEBUG_CONSOLE
@ -101,7 +102,7 @@ extern comm_settings communicationSettings;
#if (CONFIGURATION_SET == CFG_WIRESHARK)
# define OTA_HANDLER
//# define INPUT_NUNCHUK
# define INPUT_NUNCHUK
# define NUNCHUK_VCCPIN 18
# define NUNCHUK_GNDPIN 19
//# define DEBUG_CONSOLE

13
include/input.h
View File

@ -3,4 +3,15 @@
#include <Arduino.h>
void setupInput();
void loopInput( void *pvparameters );
void loopInput( void *pvparameters );
typedef enum
{
NEEDCALIB,
IDLE,
SETUP,
RUNNING,
RELEASE
} nunchuk_state;
extern nunchuk_state nunchukState;

3
include/main.h
View File

@ -26,4 +26,5 @@ extern volatile int32_t deltaMillis;
extern double plotterTempDouble[6];
#endif
void slowReset(volatile double &variable, double goal, double step, double fact);
void slowReset(volatile double &variable, double goal, double step, double fact);
void slowReset(int32_t &variable, double goal, int32_t step, double fact);

83
lib/ArduinoNunchuk/ArduinoNunchuk.cpp
View File

@ -101,7 +101,7 @@ bool ArduinoNunchuk::reInit()
pinMode(NUNCHUK_VCCPIN,INPUT);
#endif
delay(100);
delay(200);
#ifdef NUNCHUK_GNDPIN
pinMode(NUNCHUK_GNDPIN,OUTPUT);
@ -144,27 +144,20 @@ int ArduinoNunchuk::update() {
if(countFF==6) return NUNCHUK_ERR_NOINIT; // if all Bytes are FF, the Nunchuk needs to be initialised probably. Errors indicate communication problems.
if(count00==6) return NUNCHUK_ERR_ZERO; // if all Bytes are 00.
static int lastdeviationCount = 0;
// process data
// remove oldest value from sum
for(int i = 0; i<7; i++) {
avg_sum[i] -= avg_history[i][avg_ptr];
}
int16_t valuesTemp[7];
// get latest values
avg_history[0][avg_ptr] = values[0]; // analogX
avg_history[1][avg_ptr] = values[1]; // analogY
avg_history[2][avg_ptr] = ((values[2] << 2) | ((values[5] >> 2) & 3))-511; // accelX
avg_history[3][avg_ptr] = ((values[3] << 2) | ((values[5] >> 4) & 3))-511; // accelY
avg_history[4][avg_ptr] = ((values[4] << 2) | ((values[5] >> 6) & 3))-511; // accelZ
avg_history[5][avg_ptr] = !((values[5] >> 0) & 1); // zButton
avg_history[6][avg_ptr] = !((values[5] >> 1) & 1); // cButton
// add latest value to sum
for(int i = 0; i<7; i++) {
avg_sum[i] += avg_history[i][avg_ptr];
}
valuesTemp[0] = values[0]; // analogX
valuesTemp[1] = values[1]; // analogY
valuesTemp[2] = ((values[2] << 2) | ((values[5] >> 2) & 3))-511; // accelX
valuesTemp[3] = ((values[3] << 2) | ((values[5] >> 4) & 3))-511; // accelY
valuesTemp[4] = ((values[4] << 2) | ((values[5] >> 6) & 3))-511; // accelZ
valuesTemp[5] = !((values[5] >> 0) & 1); // zButton
valuesTemp[6] = !((values[5] >> 1) & 1); // cButton
// check if new values are valid
int deviationCount = 0;
@ -174,55 +167,81 @@ int ArduinoNunchuk::update() {
#endif
for(int i = 0; i<5; i++) {
avg_diff[i] = avg_history[i][avg_ptr] - ( avg_sum[i] / NUNCHUK_HISTORY );
avg_diff[i] = valuesTemp[i] - ( avg_sum[i] / NUNCHUK_HISTORY );
// if(avg_diff[i] > NUNCHUK_HIST_ANALOGTHRESH ) avg_diff[i] = NUNCHUK_HIST_ANALOGTHRESH ;
// else if(avg_diff[i] < (-NUNCHUK_HIST_ANALOGTHRESH ) ) avg_diff[i] = -NUNCHUK_HIST_ANALOGTHRESH ;
}
if( abs( avg_diff[0] ) < NUNCHUK_HIST_ANALOGTHRESH ) {
ArduinoNunchuk::analogX = avg_history[0][avg_ptr];
ArduinoNunchuk::analogX = valuesTemp[0];
} else {
deviationCount++;
}
if( abs( avg_diff[1] ) < NUNCHUK_HIST_ANALOGTHRESH ) {
ArduinoNunchuk::analogY = avg_history[1][avg_ptr];
ArduinoNunchuk::analogY = valuesTemp[1];
} else {
deviationCount++;
}
if( abs( avg_diff[2] ) < NUNCHUK_HIST_ACCELTHRESH ) {
ArduinoNunchuk::accelX = avg_history[2][avg_ptr];
ArduinoNunchuk::accelX = valuesTemp[2];
} else {
deviationCount++;
}
if( abs( avg_diff[3] ) < NUNCHUK_HIST_ACCELTHRESH ) {
ArduinoNunchuk::accelY = avg_history[3][avg_ptr];
ArduinoNunchuk::accelY = valuesTemp[3];
} else {
deviationCount++;
}
if( abs( avg_diff[4] ) < NUNCHUK_HIST_ACCELTHRESH ) {
ArduinoNunchuk::accelZ = avg_history[4][avg_ptr];
ArduinoNunchuk::accelZ = valuesTemp[4];
} else {
deviationCount++;
}
if( abs( ( avg_history[5][avg_ptr] * NUNCHUK_HISTORY ) - avg_sum[5] ) <= NUNCHUK_HIST_BUTTONTHRESH ) {
if( abs( ( valuesTemp[5] * NUNCHUK_HISTORY ) - avg_sum[5] ) <= NUNCHUK_HIST_BUTTONTHRESH ) {
zButton_last = zButton;
ArduinoNunchuk::zButton = avg_history[5][avg_ptr];
ArduinoNunchuk::zButton = valuesTemp[5];
} else {
deviationCount++;
}
if( abs( ( avg_history[6][avg_ptr] * NUNCHUK_HISTORY ) - avg_sum[6] ) <= NUNCHUK_HIST_BUTTONTHRESH ) {
if( abs( ( valuesTemp[6] * NUNCHUK_HISTORY ) - avg_sum[6] ) <= NUNCHUK_HIST_BUTTONTHRESH ) {
cButton_last = cButton;
ArduinoNunchuk::cButton = avg_history[6][avg_ptr];
ArduinoNunchuk::cButton = valuesTemp[6];
} else {
deviationCount++;
}
if(deviationCount == 0 || lastdeviationCount != 0)
{
// remove oldest value from sum
for(int i = 0; i<7; i++) {
avg_sum[i] -= avg_history[i][avg_ptr];
}
// get latest values
avg_history[0][avg_ptr] = values[0]; // analogX
avg_history[1][avg_ptr] = values[1]; // analogY
avg_history[2][avg_ptr] = ((values[2] << 2) | ((values[5] >> 2) & 3))-511; // accelX
avg_history[3][avg_ptr] = ((values[3] << 2) | ((values[5] >> 4) & 3))-511; // accelY
avg_history[4][avg_ptr] = ((values[4] << 2) | ((values[5] >> 6) & 3))-511; // accelZ
avg_history[5][avg_ptr] = !((values[5] >> 0) & 1); // zButton
avg_history[6][avg_ptr] = !((values[5] >> 1) & 1); // cButton
// add latest value to sum
for(int i = 0; i<7; i++) {
avg_sum[i] += avg_history[i][avg_ptr];
}
}
lastdeviationCount = deviationCount;
avg_ptr++;
if(NUNCHUK_HISTORY == avg_ptr) avg_ptr = 0;
@ -257,14 +276,8 @@ int ArduinoNunchuk::update(volatile double &pwm, volatile double &steer) {
yaw_zero = yawangle();
roll_zero = rollangle();
}
double newSteer = scaleAngle(rollangle() - roll_zero , 1000.0 / NUNCHUK_ACCEL_STEER_ANGLE);
double newPwm = scaleAngle(pitchangle() - pitch_zero, 1000.0 / NUNCHUK_ACCEL_SPEED_ANGLE);
newSteer = steer + limit(-70.0, newSteer - steer, 70.0);
newPwm = pwm + limit(-70.0, newPwm - pwm, 70.0);
steer = (steer * 0.5) + (0.5 * newSteer);
pwm = (pwm * 0.5) + (0.5 * newPwm);
steer = scaleAngle(rollangle() - roll_zero , 1000.0 / NUNCHUK_ACCEL_STEER_ANGLE);
pwm = scaleAngle(pitchangle() - pitch_zero, 1000.0 / NUNCHUK_ACCEL_SPEED_ANGLE);
} else if (cButton && zButton) {
/* Joystick calibration mode when both buttons are pressed */
if((zButton_last != zButton) || (cButton_last != cButton)) { // do calibration

2
lib/ArduinoNunchuk/ArduinoNunchuk.h
View File

@ -37,7 +37,7 @@
#define NUNCHUK_JOYSTICK_THRESHOLD_LOW 77 // calibration values above this are considered invalid
#define NUNCHUK_JOYSTICK_THRESHOLD_HIGH 177 // calibration values below this are considered invalid
#define NUNCHUK_JOYSTICK_STEER_MULT 0.5 // 0.8 too much
#define NUNCHUK_JOYSTICK_SPEED_MULT 1.4 // 0.5 way too slow
#define NUNCHUK_JOYSTICK_SPEED_MULT 1.0 // 0.5 way too slow
#define NUNCHUK_ACCEL_SPEED_ANGLE 60 // Pitch angle needed to reach full speed (60° with factor 0.5 was too slow)
#define NUNCHUK_ACCEL_STEER_ANGLE 100 // Pitch angle needed to reach full speed (90° with factor 0.8 was ok,little slow)
#define NUNCHUK_HISTORY 8 // Number of array size for history. Use multiples of 2 to simplify division

47
src/communication.cpp
View File

@ -289,6 +289,17 @@ void relayDataOut ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTO
void relayDataIn ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg );
void processPWMdata ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) {
switch (cmd) {
case PROTOCOL_CMD_READVAL:
case PROTOCOL_CMD_SILENTREAD:
slowReset(PWMData.pwm[0], motor.setpoint.pwm + motor.setpoint.steer, 50, 0.0);
slowReset(PWMData.pwm[1], motor.setpoint.pwm - motor.setpoint.steer, 50, 0.0);
break;
}
fn_defaultProcessing(s, param, cmd, msg);
}
void waitForMessage ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) {
switch (cmd) {
case PROTOCOL_CMD_WRITEVAL:
case PROTOCOL_CMD_READVALRESPONSE:
@ -297,34 +308,19 @@ void processPWMdata ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PRO
// Relay messages coming from hbpIn (ESP Now, remote) to hbpOut (Hoverboard, UART)
if(hbpIn.s.params[HoverboardAPI::Codes::setPointPWM]) hbpIn.updateParamHandler( HoverboardAPI::Codes::setPointPWM ,relayDataOut);
hbpOut.scheduleTransmission(HoverboardAPI::Codes::setPointPWM, 0, 30);
}
break;
case PROTOCOL_CMD_READVAL:
case PROTOCOL_CMD_SILENTREAD:
PWMData.pwm[0] = motor.setpoint.pwm + motor.setpoint.steer;
PWMData.pwm[1] = motor.setpoint.pwm - motor.setpoint.steer;
break;
}
fn_defaultProcessing(s, param, cmd, msg);
}
void processPIDSpeedData ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) {
switch (cmd) {
case PROTOCOL_CMD_WRITEVAL:
case PROTOCOL_CMD_READVALRESPONSE:
if( communicationSettings.input == COMM_IN_ESPNOW || communicationSettings.input == COMM_IN_UDP)
{
// Relay messages coming from hbpIn (ESP Now, remote) to hbpOut (Hoverboard, UART)
if(hbpIn.s.params[HoverboardAPI::Codes::setSpeed]) hbpIn.updateParamHandler( HoverboardAPI::Codes::setSpeed ,relayDataOut);
hbpOut.scheduleTransmission(HoverboardAPI::Codes::setSpeed, 0, 30);
}
break;
}
}
void processPIDSpeedData ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) {
switch (cmd) {
case PROTOCOL_CMD_READVAL:
case PROTOCOL_CMD_SILENTREAD:
PIDSpeedData.wanted_speed_mm_per_sec[0] = motor.setpoint.pwm + motor.setpoint.steer;
PIDSpeedData.wanted_speed_mm_per_sec[1] = motor.setpoint.pwm - motor.setpoint.steer;
slowReset(PIDSpeedData.wanted_speed_mm_per_sec[0], motor.setpoint.pwm + motor.setpoint.steer, 50, 0.0);
slowReset(PIDSpeedData.wanted_speed_mm_per_sec[1], motor.setpoint.pwm - motor.setpoint.steer, 50, 0.0);
break;
}
fn_defaultProcessing(s, param, cmd, msg);
@ -514,6 +510,8 @@ void hbpoutSetupPWMtransmission()
hbpOut.sendEnable(1, PROTOCOL_SOM_ACK);
hbpOut.sendEnable(1, PROTOCOL_SOM_ACK); // TODO: Check why this workaround is neccessary.
#endif
hbpIn.updateParamHandler( HoverboardAPI::Codes::setSpeed, waitForMessage);
hbpIn.updateParamHandler( HoverboardAPI::Codes::setPointPWM, waitForMessage);
}
void processOdroidGo()
@ -580,6 +578,13 @@ void processOdroidGo()
if(odroidSpeed > 400) odroidSpeed = 400;
if(odroidSpeed < -400) odroidSpeed = -400;
if(nunchukState != RUNNING)
{
slowReset(motor.setpoint.pwm, odroidSpeed, 0, 0.15);
slowReset(motor.setpoint.steer, odroidSteer, 0, 0.5);
}
if(GO.BtnStart.isPressed()) hbpOut.sendPing();
#ifdef DEBUG_SPEED

2
src/hoverboardAPI

@ -1 +1 @@
Subproject commit f77da4f9bca23c5207a49be724d577652226c083
Subproject commit d5f2ef26dee696ba16453fb29f65f2cb8dc3725e

59
src/input.cpp
View File

@ -39,6 +39,7 @@
#endif
uint32_t millisMotorcomm = 0; // virtual timer for motor update
nunchuk_state nunchukState = NEEDCALIB;
void setupInput() {
@ -68,10 +69,10 @@ void setupInput() {
typedef enum
{
NUNCHUK_OK = 0x00,
NUNCHUK_MINORERR = 0x01,
NUNCHUK_NOK = 0x02,
NUNCHUK_NOINIT = 0x03,
NUNCHUK_OK,
NUNCHUK_MINORERR,
NUNCHUK_NOK,
NUNCHUK_NOINIT,
} nunchuk_poll_status;
typedef struct
@ -203,19 +204,6 @@ void loopInput( void *pvparameters ) {
#if defined(INPUT_NUNCHUK)
typedef enum
{
GOT_DATA,
REINIT,
NO_DATA,
NUNCHUK_NOINIT,
IDLE,
SETUP,
RUNNING,
RELEASE
} nunchuk_state;
static nunchuk_state nunchukState = IDLE;
static int nunchukReinitCount=0;
static int nunchukTimeout=0;
@ -233,6 +221,9 @@ void loopInput( void *pvparameters ) {
nunchukReinitCount = 1000;
break;
case NUNCHUK_MINORERR:
break;
default:
++nunchukTimeout;
++nunchukReinitCount;
@ -241,7 +232,7 @@ void loopInput( void *pvparameters ) {
// try fixing the Nunchuk by resetting
if(nunchukReinitCount>=5)
if(nunchukReinitCount>=10)
{
nunchuk.reInit();
if(debug) COM[DEBUG_COM]->print("Reinit Nunchuk ");
@ -251,6 +242,21 @@ void loopInput( void *pvparameters ) {
switch (nunchukState)
{
case NEEDCALIB:
if(poll.status == NUNCHUK_OK)
{
nunchuk.analogX_zero = nunchuk.analogX;
nunchuk.analogY_zero = nunchuk.analogY;
nunchuk.analogX_min = 0;
nunchuk.analogX_max = 255;
nunchuk.analogY_min = 0;
nunchuk.analogY_max = 255;
delay(1000);
nunchukState = SETUP;
}
break;
case IDLE:
if(poll.status == NUNCHUK_OK) nunchukState = SETUP;
break;
@ -263,18 +269,19 @@ void loopInput( void *pvparameters ) {
if(poll.status == NUNCHUK_OK)
{
motor.setpoint.pwm = poll.pwm;
motor.setpoint.steer = poll.steer;
slowReset(motor.setpoint.pwm, poll.pwm, 20, 0);
slowReset(motor.setpoint.steer, poll.steer, 20, 0);
}
// set safe value when no data is received for a long time
if(nunchukTimeout>=10) nunchukState = RELEASE;
if(nunchukTimeout>=20) nunchukState = RELEASE;
break;
case RELEASE: //no new data, timeout
// protocol recovers on its own as soon as data is received
motor.setpoint.pwm = 0.0;
motor.setpoint.steer = 0.0;
nunchukState = IDLE;
break;
default:
@ -293,11 +300,6 @@ void loopInput( void *pvparameters ) {
if(debug) paddelec.debug(*COM[DEBUG_COM]);
#endif
#ifdef ODROID_GO_HW
slowReset(motor.setpoint.pwm, odroidSpeed, 0, 0.15);
slowReset(motor.setpoint.steer, odroidSteer, 0, 0.5);
#endif
} while(false);
#if defined(DEBUG_PLOTTER) && defined(INPUT_PADDELECIMU)
@ -376,6 +378,11 @@ void loopInput( void *pvparameters ) {
} while( u8g2.nextPage() );
#endif
#ifdef INPUT_NUNCHUK
vTaskDelay(1);
#else
vTaskDelay(10);
#endif
}
}

7
src/main.cpp
View File

@ -97,4 +97,11 @@ void slowReset(volatile double &variable, double goal, double step, double fact)
if ((variable - goal) > step) variable -= step;
else if ((goal - variable) > step) variable += step;
else variable = goal;
}
void slowReset(int32_t &variable, double goal, int32_t step, double fact) {
variable += (int32_t)((goal - variable) * fact);
if ((variable - goal) > step) variable -= step;
else if ((goal - variable) > step) variable += step;
else variable = goal;
}