interfisch
/
hoverboard-firmware-hack-foc-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

InputStruct update 

- input1, input2 converted to structure
- some functions are re-positioned in util.c
This commit is contained in:
EmanuelFeru 2020-12-13 21:52:29 +01:00
parent 9504845ec1
commit 5ca3fa4f85
4 changed files with 516 additions and 500 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Inc/config.h
View File

@ -120,7 +120,7 @@
* 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast! * 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast!
Inputs: Inputs:
- cmd1 and cmd2: analog normalized input values. INPUT_MIN to INPUT_MAX - input1.cmd and input2.cmd: analog normalized input values. INPUT_MIN to INPUT_MAX
- button1 and button2: digital input values. 0 or 1 - button1 and button2: digital input values. 0 or 1
- adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095 - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
Outputs: Outputs:
@ -212,8 +212,8 @@
* *
* in1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS * in1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS
* in2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS * in2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS
* cmdL: (int16_t)speedL); output command: [-1000, 1000] * cmdL: (int16_t)cmdL); output command: [-1000, 1000]
* cmdR: (int16_t)speedR); output command: [-1000, 1000] * cmdR: (int16_t)cmdR); output command: [-1000, 1000]
* BatADC: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard * BatADC: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard
* BatV: (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); Battery calibrated voltage multiplied by 100 for verifying battery voltage calibration * BatV: (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); Battery calibrated voltage multiplied by 100 for verifying battery voltage calibration
* TempADC: (int16_t)board_temp_adcFilt); for board temperature calibration * TempADC: (int16_t)board_temp_adcFilt); for board temperature calibration

26
Inc/util.h
View File

@ -55,6 +55,16 @@
} SerialSideboard; } SerialSideboard;
#endif #endif
typedef struct {
int16_t raw; // raw input
int16_t cmd; // command (scaled)
uint8_t typ;
int16_t min;
int16_t mid;
int16_t max;
int16_t deadband;
} InputStruct;
// Initialization Functions // Initialization Functions
void BLDC_Init(void); void BLDC_Init(void);
void Input_Lim_Init(void); void Input_Lim_Init(void);
@ -68,21 +78,16 @@ void beepLong(uint8_t freq);
void beepShort(uint8_t freq); void beepShort(uint8_t freq);
void beepShortMany(uint8_t cnt, int8_t dir); void beepShortMany(uint8_t cnt, int8_t dir);
void calcAvgSpeed(void); void calcAvgSpeed(void);
int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max);
int checkInputType(int16_t min, int16_t mid, int16_t max);
void adcCalibLim(void); void adcCalibLim(void);
void updateCurSpdLim(void); void updateCurSpdLim(void);
void saveConfig(void);
void standstillHold(void); void standstillHold(void);
void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
void cruiseControl(uint8_t button); void cruiseControl(uint8_t button);
int checkInputType(int16_t min, int16_t mid, int16_t max);
// Poweroff Functions
void poweroff(void);
void poweroffPressCheck(void);
// Read Functions // Read Functions
void readInput(void); void readInputRaw(void);
void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max);
void readCommand(void); void readCommand(void);
void usart2_rx_check(void); void usart2_rx_check(void);
void usart3_rx_check(void); void usart3_rx_check(void);
@ -100,6 +105,11 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid
void sideboardLeds(uint8_t *leds); void sideboardLeds(uint8_t *leds);
void sideboardSensors(uint8_t sensors); void sideboardSensors(uint8_t sensors);
// Poweroff Functions
void saveConfig(void);
void poweroff(void);
void poweroffPressCheck(void);
// Filtering Functions // Filtering Functions
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y); void filtLowPass32(int32_t u, uint16_t coef, int32_t *y);
void rateLimiter16(int16_t u, int16_t rate, int16_t *y); void rateLimiter16(int16_t u, int16_t rate, int16_t *y);

56
Src/main.c
View File

@ -62,10 +62,8 @@ extern ExtY rtY_Left; /* External outputs */
extern ExtY rtY_Right; /* External outputs */ extern ExtY rtY_Right; /* External outputs */
//--------------- //---------------
extern int16_t cmd1; // normalized input value. -1000 to 1000 extern InputStruct input1; // input structure
extern int16_t cmd2; // normalized input value. -1000 to 1000 extern InputStruct input2; // input structure
extern int16_t input1; // Non normalized input value
extern int16_t input2; // Non normalized input value
extern int16_t speedAvg; // Average measured speed extern int16_t speedAvg; // Average measured speed
extern int16_t speedAvgAbs; // Average measured speed in absolute extern int16_t speedAvgAbs; // Average measured speed in absolute
@ -191,7 +189,7 @@ int main(void) {
HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET); HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
int16_t cmdL = 0, cmdR = 0; int16_t cmdL = 0, cmdR = 0;
int16_t lastCmdL = 0, lastCmdR = 0; int16_t cmdL_prev = 0, cmdR_prev = 0;
int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point
int16_t board_temp_adcFilt = adc_buffer.temp; int16_t board_temp_adcFilt = adc_buffer.temp;
@ -201,12 +199,12 @@ int main(void) {
while(1) { while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); // delay in ms HAL_Delay(DELAY_IN_MAIN_LOOP); // delay in ms
readCommand(); // Read Command: cmd1, cmd2 readCommand(); // Read Command: input1.cmd, input2.cmd
calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs
#ifndef VARIANT_TRANSPOTTER #ifndef VARIANT_TRANSPOTTER
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (input1.cmd > -50 && input1.cmd < 50) && (input2.cmd > -50 && input2.cmd < 50)){
beepShort(6); // make 2 beeps indicating the motor enable beepShort(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100); beepShort(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters steerFixdt = speedFixdt = 0; // reset filters
@ -228,11 +226,11 @@ int main(void) {
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
if (speedAvgAbs < 60) { // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality if (speedAvgAbs < 60) { // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality
multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "cmd1" variable multipleTapDet(input1.cmd, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "input1" variable
} }
if (cmd1 > 30) { // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving if (input1.cmd > 30) { // If Brake pedal (input1) is pressed, bring to 0 also the Throttle pedal (input2) to avoid "Double pedal" driving
cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); input2.cmd = (int16_t)((input2.cmd * speedBlend) >> 15);
cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna); // Cruise control deactivated by Brake pedal if it was active cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna); // Cruise control deactivated by Brake pedal if it was active
} }
#endif #endif
@ -243,25 +241,25 @@ int main(void) {
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
if (speedAvg > 0) { // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal) if (speedAvg > 0) { // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal)
cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15); input1.cmd = (int16_t)((-input1.cmd * speedBlend) >> 15);
} else { } else {
cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15); input1.cmd = (int16_t)(( input1.cmd * speedBlend) >> 15);
} }
#endif #endif
#ifdef VARIANT_SKATEBOARD #ifdef VARIANT_SKATEBOARD
if (cmd2 < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving) if (input2.cmd < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving)
if (speedAvg > 0) { // Make sure the braking is opposite to the direction of motion if (speedAvg > 0) { // Make sure the braking is opposite to the direction of motion
cmd2 = (int16_t)(( cmd2 * speedBlend) >> 15); input2.cmd = (int16_t)(( input2.cmd * speedBlend) >> 15);
} else { } else {
cmd2 = (int16_t)((-cmd2 * speedBlend) >> 15); input2.cmd = (int16_t)((-input2.cmd * speedBlend) >> 15);
} }
} }
#endif #endif
// ####### LOW-PASS FILTER ####### // ####### LOW-PASS FILTER #######
rateLimiter16(cmd1, RATE, &steerRateFixdt); rateLimiter16(input1.cmd , RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt); rateLimiter16(input2.cmd , RATE, &speedRateFixdt);
filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer
@ -277,12 +275,12 @@ int main(void) {
#endif #endif
// ####### MIXER ####### // ####### MIXER #######
// cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); // cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX);
// cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); // cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX);
mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above
// ####### SET OUTPUTS (if the target change is less than +/- 100) ####### // ####### SET OUTPUTS (if the target change is less than +/- 100) #######
if ((cmdL > lastCmdL-100 && cmdL < lastCmdL+100) && (cmdR > lastCmdR-100 && cmdR < lastCmdR+100)) { if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {
#ifdef INVERT_R_DIRECTION #ifdef INVERT_R_DIRECTION
pwmr = cmdR; pwmr = cmdR;
#else #else
@ -297,14 +295,14 @@ int main(void) {
#endif #endif
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
distance = CLAMP(cmd1 - 180, 0, 4095); distance = CLAMP(input1.cmd - 180, 0, 4095);
steering = (cmd2 - 2048) / 2048.0; steering = (input2.cmd - 2048) / 2048.0;
distanceErr = distance - (int)(setDistance * 1345); distanceErr = distance - (int)(setDistance * 1345);
if (nunchuk_connected == 0) { if (nunchuk_connected == 0) {
cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f); cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f); cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f);
if ((cmdL < lastCmdL + 50 && cmdL > lastCmdL - 50) && (cmdR < lastCmdR + 50 && cmdR > lastCmdR - 50)) { if ((cmdL < cmdL_prev + 50 && cmdL > cmdL_prev - 50) && (cmdR < cmdR_prev + 50 && cmdR > cmdR_prev - 50)) {
if (distanceErr > 0) { if (distanceErr > 0) {
enable = 1; enable = 1;
} }
@ -412,8 +410,8 @@ int main(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms
printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n", printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n",
input1, // 1: INPUT1 input1.raw, // 1: INPUT1
input2, // 2: INPUT2 input2.raw, // 2: INPUT2
cmdL, // 3: output command: [-1000, 1000] cmdL, // 3: output command: [-1000, 1000]
cmdR, // 4: output command: [-1000, 1000] cmdR, // 4: output command: [-1000, 1000]
adc_buffer.batt1, // 5: for battery voltage calibration adc_buffer.batt1, // 5: for battery voltage calibration
@ -427,8 +425,8 @@ int main(void) {
#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) #if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
if (main_loop_counter % 2 == 0) { // Send data periodically every 10 ms if (main_loop_counter % 2 == 0) { // Send data periodically every 10 ms
Feedback.start = (uint16_t)SERIAL_START_FRAME; Feedback.start = (uint16_t)SERIAL_START_FRAME;
Feedback.cmd1 = (int16_t)cmd1; Feedback.cmd1 = (int16_t)input1.cmd;
Feedback.cmd2 = (int16_t)cmd2; Feedback.cmd2 = (int16_t)input2.cmd;
Feedback.speedR_meas = (int16_t)rtY_Right.n_mot; Feedback.speedR_meas = (int16_t)rtY_Right.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Left.n_mot; Feedback.speedL_meas = (int16_t)rtY_Left.n_mot;
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
@ -497,8 +495,8 @@ int main(void) {
// HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN // HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN
// Update main loop states // Update main loop states
lastCmdL = cmdL; cmdL_prev = cmdL;
lastCmdR = cmdR; cmdR_prev = cmdR;
main_loop_counter++; main_loop_counter++;
timeoutCnt++; timeoutCnt++;
} }

884
Src/util.c
View File

@ -87,10 +87,8 @@ ExtU rtU_Right; /* External inputs */
ExtY rtY_Right; /* External outputs */ ExtY rtY_Right; /* External outputs */
//--------------- //---------------
int16_t cmd1; // normalized input value. -1000 to 1000 InputStruct input1; // input structure
int16_t cmd2; // normalized input value. -1000 to 1000 InputStruct input2; // input structure
int16_t input1; // Non normalized input value
int16_t input2; // Non normalized input value
int16_t speedAvg; // average measured speed int16_t speedAvg; // average measured speed
int16_t speedAvgAbs; // average measured speed in absolute int16_t speedAvgAbs; // average measured speed in absolute
@ -132,18 +130,10 @@ static int16_t INPUT_MIN; // [-] Input target minimum limitation
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
static uint8_t cur_spd_valid = 0; static uint8_t cur_spd_valid = 0;
static uint8_t inp_cal_valid = 0; static uint8_t inp_cal_valid = 0;
static uint16_t INPUT1_TYP_CAL = INPUT1_TYPE;
static uint16_t INPUT1_MIN_CAL = INPUT1_MIN;
static uint16_t INPUT1_MID_CAL = INPUT1_MID;
static uint16_t INPUT1_MAX_CAL = INPUT1_MAX;
static uint16_t INPUT2_TYP_CAL = INPUT2_TYPE;
static uint16_t INPUT2_MIN_CAL = INPUT2_MIN;
static uint16_t INPUT2_MID_CAL = INPUT2_MID;
static uint16_t INPUT2_MAX_CAL = INPUT2_MAX;
#endif #endif
#if defined(CONTROL_ADC) #if defined(CONTROL_ADC)
static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection static uint16_t timeoutCntADC = 0; // Timeout counter for ADC Protection
#endif #endif
#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
@ -302,28 +292,32 @@ void Input_Init(void) {
#endif #endif
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
uint16_t writeCheck, i_max, n_max; uint16_t writeCheck, readVal;
HAL_FLASH_Unlock(); HAL_FLASH_Unlock();
EE_Init(); /* EEPROM Init */ EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &writeCheck); EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
if (writeCheck == FLASH_WRITE_KEY) { if (writeCheck == FLASH_WRITE_KEY) {
EE_ReadVariable(VirtAddVarTab[1] , &INPUT1_TYP_CAL); EE_ReadVariable(VirtAddVarTab[1] , &readVal); input1.typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[2] , &INPUT1_MIN_CAL); EE_ReadVariable(VirtAddVarTab[2] , &readVal); input1.min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[3] , &INPUT1_MID_CAL); EE_ReadVariable(VirtAddVarTab[3] , &readVal); input1.mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[4] , &INPUT1_MAX_CAL); EE_ReadVariable(VirtAddVarTab[4] , &readVal); input1.max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[5] , &INPUT2_TYP_CAL); EE_ReadVariable(VirtAddVarTab[5] , &readVal); input2.typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[6] , &INPUT2_MIN_CAL); EE_ReadVariable(VirtAddVarTab[6] , &readVal); input2.min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[7] , &INPUT2_MID_CAL); EE_ReadVariable(VirtAddVarTab[7] , &readVal); input2.mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[8] , &INPUT2_MAX_CAL); EE_ReadVariable(VirtAddVarTab[8] , &readVal); input2.max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[9] , &i_max); EE_ReadVariable(VirtAddVarTab[9] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[10], &n_max); EE_ReadVariable(VirtAddVarTab[10], &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal;
rtP_Left.i_max = i_max;
rtP_Left.n_max = n_max;
rtP_Right.i_max = i_max;
rtP_Right.n_max = n_max;
} else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h } else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h
if (INPUT1_TYPE == 3) { INPUT1_TYP_CAL = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); } input1.typ = INPUT1_TYPE;
if (INPUT2_TYPE == 3) { INPUT2_TYP_CAL = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); } input1.min = INPUT1_MIN;
input1.mid = INPUT1_MID;
input1.max = INPUT1_MAX;
input2.typ = INPUT2_TYPE;
input2.min = INPUT2_MIN;
input2.mid = INPUT2_MID;
input2.max = INPUT2_MAX;
if (INPUT1_TYPE == 3) { input1.typ = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); }
if (INPUT2_TYPE == 3) { input2.typ = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); }
} }
HAL_FLASH_Lock(); HAL_FLASH_Lock();
#endif #endif
@ -458,26 +452,244 @@ void calcAvgSpeed(void) {
} }
/* /*
* Add Dead-band to a signal * Auto-calibration of the ADC Limits
* This function realizes a dead-band around 0 and scales the input between [out_min, out_max] * This function finds the Minimum, Maximum, and Middle for the ADC input
* Procedure:
* - press the power button for more than 5 sec and release after the beep sound
* - move the potentiometers freely to the min and max limits repeatedly
* - release potentiometers to the resting postion
* - press the power button to confirm or wait for the 20 sec timeout
* The Values will be saved to flash. Values are persistent if you flash with platformio. To erase them, make a full chip erase.
*/ */
int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max) { void adcCalibLim(void) {
switch (type){ if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
case 0: // Input is ignored return;
return 0; }
case 1: // Input is a normal pot
return CLAMP(MAP(u, in_min, in_max, 0, out_max), 0, out_max); #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
case 2: // Input is a mid resting pot
if( u > in_mid - deadBand && u < in_mid + deadBand ) { #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
return 0; printf("Input calibration started...\r\n");
} else if(u > in_mid) { #endif
return CLAMP(MAP(u, in_mid + deadBand, in_max, 0, out_max), 0, out_max);
readInputRaw();
// Inititalization: MIN = a high value, MAX = a low value
int32_t input1_fixdt = input1.raw << 16;
int32_t input2_fixdt = input2.raw << 16;
int16_t INPUT1_MIN_temp = MAX_int16_T;
int16_t INPUT1_MID_temp = 0;
int16_t INPUT1_MAX_temp = MIN_int16_T;
int16_t INPUT2_MIN_temp = MAX_int16_T;
int16_t INPUT2_MID_temp = 0;
int16_t INPUT2_MAX_temp = MIN_int16_T;
uint16_t input_cal_timeout = 0;
// Extract MIN, MAX and MID from ADC while the power button is not pressed
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
readInputRaw();
filtLowPass32(input1.raw, FILTER, &input1_fixdt);
filtLowPass32(input2.raw, FILTER, &input2_fixdt);
INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer
INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX);
INPUT1_MIN_temp = MIN(INPUT1_MIN_temp, INPUT1_MID_temp);
INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp);
INPUT2_MIN_temp = MIN(INPUT2_MIN_temp, INPUT2_MID_temp);
INPUT2_MAX_temp = MAX(INPUT2_MAX_temp, INPUT2_MID_temp);
HAL_Delay(5);
}
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input1 is ");
#endif
input1.typ = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
if (input1.typ == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input1.min = INPUT1_MIN_temp + INPUT_MARGIN;
input1.mid = INPUT1_MID_temp;
input1.max = INPUT1_MAX_temp - INPUT_MARGIN;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else { } else {
return CLAMP(MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0); input1.typ = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
} }
default:
return 0; #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input2 is ");
#endif
input2.typ = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (input2.typ == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input2.min = INPUT2_MIN_temp + INPUT_MARGIN;
input2.mid = INPUT2_MID_temp;
input2.max = INPUT2_MAX_temp - INPUT_MARGIN;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
input2.typ = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
} }
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Limits Input1: TYP:%i MIN:%i MID:%i MAX:%i\r\nLimits Input2: TYP:%i MIN:%i MID:%i MAX:%i\r\n",
input1.typ, input1.min, input1.mid, input1.max,
input2.typ, input2.min, input2.mid, input2.max);
#endif
#endif
}
/*
* Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2)
* Procedure:
* - press the power button for more than 5 sec and immediatelly after the beep sound press one more time shortly
* - move and hold the pots to a desired limit position for Current and Speed
* - press the power button to confirm or wait for the 10 sec timeout
*/
void updateCurSpdLim(void) {
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
return;
}
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Torque and Speed limits update started...\r\n");
#endif
int32_t input1_fixdt = input1.raw << 16;
int32_t input2_fixdt = input2.raw << 16;
uint16_t cur_factor; // fixdt(0,16,16)
uint16_t spd_factor; // fixdt(0,16,16)
uint16_t cur_spd_timeout = 0;
cur_spd_valid = 0;
// Wait for the power button press
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
readInputRaw();
filtLowPass32(input1.raw, FILTER, &input1_fixdt);
filtLowPass32(input2.raw, FILTER, &input2_fixdt);
HAL_Delay(5);
}
// Calculate scaling factors
cur_factor = CLAMP((input1_fixdt - (input1.min << 16)) / (input1.max - input1.min), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
spd_factor = CLAMP((input2_fixdt - (input2.min << 16)) / (input2.max - input2.min), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
if (input1.typ != 0){
// Update current limit
rtP_Left.i_max = rtP_Right.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid = 1; // Mark update to be saved in Flash at shutdown
}
if (input2.typ != 0){
// Update speed limit
rtP_Left.n_max = rtP_Right.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown
}
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
// cur_spd_valid: 0 = No limit changed, 1 = Current limit changed, 2 = Speed limit changed, 3 = Both limits changed
printf("Limits (%i)\r\nCurrent: fixdt:%li factor%i i_max:%i \r\nSpeed: fixdt:%li factor:%i n_max:%i\r\n",
cur_spd_valid, input1_fixdt, cur_factor, rtP_Left.i_max, input2_fixdt, spd_factor, rtP_Left.n_max);
#endif
#endif
}
/*
* Standstill Hold Function
* This function uses Cruise Control to provide an anti-roll functionality at standstill.
* Only available and makes sense for FOC VOLTAGE or FOC TORQUE mode.
*
* Input: none
* Output: standstillAcv
*/
void standstillHold(void) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE)
if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation
if (((input1.cmd > 50 || input2.cmd < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (input2.cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
rtP_Left.n_cruiseMotTgt = 0;
rtP_Right.n_cruiseMotTgt = 0;
rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1;
standstillAcv = 1;
}
}
else { // If Stanstill is Active -> try Deactivation
if (input1.cmd < 20 && input2.cmd > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
standstillAcv = 0;
}
}
#endif
}
/*
* Electric Brake Function
* In case of TORQUE mode, this function replaces the motor "freewheel" with a constant braking when the input torque request is 0.
* This is useful when a small amount of motor braking is desired instead of "freewheel".
*
* Input: speedBlend = fixdt(0,16,15), reverseDir = {0, 1}
* Output: input2.cmd (Throtle) with brake component included
*/
void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
#if defined(ELECTRIC_BRAKE_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ == TRQ_MODE)
int16_t brakeVal;
// Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving)
if (speedAvg > 0) {
brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
} else {
brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
}
// Check if direction is reversed
if (reverseDir) {
brakeVal = -brakeVal;
}
// Calculate the new input2.cmd with brake component included
if (input2.cmd >= 0 && input2.cmd < ELECTRIC_BRAKE_THRES) {
input2.cmd = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - input2.cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2.cmd >= -ELECTRIC_BRAKE_THRES && input2.cmd < 0) {
input2.cmd = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + input2.cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2.cmd >= ELECTRIC_BRAKE_THRES) {
input2.cmd = MAX(brakeVal, ((input2.cmd - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES));
} else { // when (input2.cmd < -ELECTRIC_BRAKE_THRES)
input2.cmd = MIN(brakeVal, ((input2.cmd + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES));
}
#endif
}
/*
* Cruise Control Function
* This function activates/deactivates cruise control.
*
* Input: button (as a pulse)
* Output: cruiseCtrlAcv
*/
void cruiseControl(uint8_t button) {
#ifdef CRUISE_CONTROL_SUPPORT
if (button && !rtP_Left.b_cruiseCtrlEna) { // Cruise control activated
rtP_Left.n_cruiseMotTgt = rtY_Left.n_mot;
rtP_Right.n_cruiseMotTgt = rtY_Right.n_mot;
rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1;
cruiseCtrlAcv = 1;
beepShortMany(2, 1); // 200 ms beep delay. Acts as a debounce also.
} else if (button && rtP_Left.b_cruiseCtrlEna && !standstillAcv) { // Cruise control deactivated if no Standstill Hold is active
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
cruiseCtrlAcv = 0;
beepShortMany(2, -1);
}
#endif
} }
/* /*
@ -524,364 +736,26 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){
return type; return type;
} }
/*
* Auto-calibration of the ADC Limits
* This function finds the Minimum, Maximum, and Middle for the ADC input
* Procedure:
* - press the power button for more than 5 sec and release after the beep sound
* - move the potentiometers freely to the min and max limits repeatedly
* - release potentiometers to the resting postion
* - press the power button to confirm or wait for the 20 sec timeout
* The Values will be saved to flash. Values are persistent if you flash with platformio. To erase them, make a full chip erase.
*/
void adcCalibLim(void) {
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
return;
}
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input calibration started...\r\n");
#endif
readInput();
// Inititalization: MIN = a high value, MAX = a low value
int32_t input1_fixdt = input1 << 16;
int32_t input2_fixdt = input2 << 16;
int16_t INPUT1_MIN_temp = MAX_int16_T;
int16_t INPUT1_MID_temp = 0;
int16_t INPUT1_MAX_temp = MIN_int16_T;
int16_t INPUT2_MIN_temp = MAX_int16_T;
int16_t INPUT2_MID_temp = 0;
int16_t INPUT2_MAX_temp = MIN_int16_T;
uint16_t input_cal_timeout = 0;
// Extract MIN, MAX and MID from ADC while the power button is not pressed
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
readInput();
filtLowPass32(input1, FILTER, &input1_fixdt);
filtLowPass32(input2, FILTER, &input2_fixdt);
INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer
INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX);
INPUT1_MIN_temp = MIN(INPUT1_MIN_temp, INPUT1_MID_temp);
INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp);
INPUT2_MIN_temp = MIN(INPUT2_MIN_temp, INPUT2_MID_temp);
INPUT2_MAX_temp = MAX(INPUT2_MAX_temp, INPUT2_MID_temp);
HAL_Delay(5);
}
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input1 is ");
#endif
INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
if (INPUT1_TYP_CAL == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN;
INPUT1_MID_CAL = INPUT1_MID_temp;
INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
INPUT1_TYP_CAL = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
}
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input2 is ");
#endif
INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (INPUT2_TYP_CAL == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN;
INPUT2_MID_CAL = INPUT2_MID_temp;
INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
INPUT2_TYP_CAL = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
}
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Limits Input1: TYP:%i MIN:%i MID:%i MAX:%i\r\nLimits Input2: TYP:%i MIN:%i MID:%i MAX:%i\r\n",
INPUT1_TYP_CAL, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL,
INPUT2_TYP_CAL, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL);
#endif
#endif
}
/*
* Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2)
* Procedure:
* - press the power button for more than 5 sec and immediatelly after the beep sound press one more time shortly
* - move and hold the pots to a desired limit position for Current and Speed
* - press the power button to confirm or wait for the 10 sec timeout
*/
void updateCurSpdLim(void) {
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
return;
}
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Torque and Speed limits update started...\r\n");
#endif
int32_t input1_fixdt = input1 << 16;
int32_t input2_fixdt = input2 << 16;
uint16_t cur_factor; // fixdt(0,16,16)
uint16_t spd_factor; // fixdt(0,16,16)
uint16_t cur_spd_timeout = 0;
cur_spd_valid = 0;
// Wait for the power button press
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
readInput();
filtLowPass32(input1, FILTER, &input1_fixdt);
filtLowPass32(input2, FILTER, &input2_fixdt);
HAL_Delay(5);
}
// Calculate scaling factors
cur_factor = CLAMP((input1_fixdt - ((int16_t)INPUT1_MIN_CAL << 16)) / ((int16_t)INPUT1_MAX_CAL - (int16_t)INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
spd_factor = CLAMP((input2_fixdt - ((int16_t)INPUT2_MIN_CAL << 16)) / ((int16_t)INPUT2_MAX_CAL - (int16_t)INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
if (INPUT1_TYP_CAL != 0){
// Update current limit
rtP_Left.i_max = rtP_Right.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid = 1; // Mark update to be saved in Flash at shutdown
}
if (INPUT2_TYP_CAL != 0){
// Update speed limit
rtP_Left.n_max = rtP_Right.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown
}
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
// cur_spd_valid: 0 = No limit changed, 1 = Current limit changed, 2 = Speed limit changed, 3 = Both limits changed
printf("Limits (%i)\r\nCurrent: fixdt:%li factor%i i_max:%i \r\nSpeed: fixdt:%li factor:%i n_max:%i\r\n",
cur_spd_valid, input1_fixdt, cur_factor, rtP_Left.i_max, input2_fixdt, spd_factor, rtP_Left.n_max);
#endif
#endif
}
/*
* Save Configuration to Flash
* This function makes sure data is not lost after power-off
*/
void saveConfig() {
#ifdef VARIANT_TRANSPOTTER
if (saveValue_valid) {
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0], saveValue);
HAL_FLASH_Lock();
}
#endif
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if (inp_cal_valid || cur_spd_valid) {
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0] , FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1] , INPUT1_TYP_CAL);
EE_WriteVariable(VirtAddVarTab[2] , INPUT1_MIN_CAL);
EE_WriteVariable(VirtAddVarTab[3] , INPUT1_MID_CAL);
EE_WriteVariable(VirtAddVarTab[4] , INPUT1_MAX_CAL);
EE_WriteVariable(VirtAddVarTab[5] , INPUT2_TYP_CAL);
EE_WriteVariable(VirtAddVarTab[6] , INPUT2_MIN_CAL);
EE_WriteVariable(VirtAddVarTab[7] , INPUT2_MID_CAL);
EE_WriteVariable(VirtAddVarTab[8] , INPUT2_MAX_CAL);
EE_WriteVariable(VirtAddVarTab[9] , rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[10], rtP_Left.n_max);
HAL_FLASH_Lock();
}
#endif
}
/*
* Standstill Hold Function
* This function uses Cruise Control to provide an anti-roll functionality at standstill.
* Only available and makes sense for FOC VOLTAGE or FOC TORQUE mode.
*
* Input: none
* Output: standstillAcv
*/
void standstillHold(void) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE)
if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation
if (((cmd1 > 50 || cmd2 < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (cmd2 < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
rtP_Left.n_cruiseMotTgt = 0;
rtP_Right.n_cruiseMotTgt = 0;
rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1;
standstillAcv = 1;
}
}
else { // If Stanstill is Active -> try Deactivation
if (cmd1 < 20 && cmd2 > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
standstillAcv = 0;
}
}
#endif
}
/*
* Electric Brake Function
* In case of TORQUE mode, this function replaces the motor "freewheel" with a constant braking when the input torque request is 0.
* This is useful when a small amount of motor braking is desired instead of "freewheel".
*
* Input: speedBlend = fixdt(0,16,15), reverseDir = {0, 1}
* Output: cmd2 (Throtle) with brake component included
*/
void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
#if defined(ELECTRIC_BRAKE_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ == TRQ_MODE)
int16_t brakeVal;
// Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving)
if (speedAvg > 0) {
brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
} else {
brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
}
// Check if direction is reversed
if (reverseDir) {
brakeVal = -brakeVal;
}
// Calculate the new cmd2 with brake component included
if (cmd2 >= 0 && cmd2 < ELECTRIC_BRAKE_THRES) {
cmd2 = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - cmd2) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (cmd2 >= -ELECTRIC_BRAKE_THRES && cmd2 < 0) {
cmd2 = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + cmd2) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (cmd2 >= ELECTRIC_BRAKE_THRES) {
cmd2 = MAX(brakeVal, ((cmd2 - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES));
} else { // when (cmd2 < -ELECTRIC_BRAKE_THRES)
cmd2 = MIN(brakeVal, ((cmd2 + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES));
}
#endif
}
/*
* Cruise Control Function
* This function activates/deactivates cruise control.
*
* Input: button (as a pulse)
* Output: cruiseCtrlAcv
*/
void cruiseControl(uint8_t button) {
#ifdef CRUISE_CONTROL_SUPPORT
if (button && !rtP_Left.b_cruiseCtrlEna) { // Cruise control activated
rtP_Left.n_cruiseMotTgt = rtY_Left.n_mot;
rtP_Right.n_cruiseMotTgt = rtY_Right.n_mot;
rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1;
cruiseCtrlAcv = 1;
shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also.
} else if (button && rtP_Left.b_cruiseCtrlEna && !standstillAcv) { // Cruise control deactivated if no Standstill Hold is active
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
cruiseCtrlAcv = 0;
shortBeepMany(2, -1);
}
#endif
}
/* =========================== Poweroff Functions =========================== */
void poweroff(void) {
enable = 0;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("-- Motors disabled --\r\n");
#endif
buzzerCount = 0; // prevent interraction with beep counter
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
saveConfig();
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
while(1) {}
}
void poweroffPressCheck(void) {
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
uint16_t cnt_press = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
HAL_Delay(10);
if (cnt_press++ == 5 * 100) { beepShort(5); }
}
if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec
HAL_Delay(1000);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
beepLong(8);
updateCurSpdLim();
beepShort(5);
} else { // Long press: Calibrate ADC Limits
beepLong(16);
adcCalibLim();
beepShort(5);
}
} else { // Short press: power off
poweroff();
}
}
#elif defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
beepShort(5);
HAL_Delay(300);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
beepLong(5);
HAL_Delay(350);
poweroff();
} else {
setDistance += 0.25;
if (setDistance > 2.6) {
setDistance = 0.5;
}
beepShort(setDistance / 0.25);
saveValue = setDistance * 1000;
saveValue_valid = 1;
}
}
#else
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0; // disable motors
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released
poweroff(); // release power-latch
}
#endif
}
/* =========================== Read Functions =========================== */ /* =========================== Read Functions =========================== */
/* /*
* Function to read the raw Input values from various input devices * Function to read the Input Raw values from various input devices
*/ */
void readInput(void) { void readInputRaw(void) {
#ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h
input1.raw = adc_buffer.l_tx2;
input2.raw = adc_buffer.l_rx2;
timeoutCnt = 0;
#endif
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
if (nunchuk_connected != 0) { if (nunchuk_connected != 0) {
Nunchuk_Read(); Nunchuk_Read();
input1 = (nunchuk_data[0] - 127) * 8; // X axis 0-255 input1.raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255
input2 = (nunchuk_data[1] - 128) * 8; // Y axis 0-255 input2.raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255
#ifdef SUPPORT_BUTTONS #ifdef SUPPORT_BUTTONS
button1 = (uint8_t)nunchuk_data[5] & 1; button1 = (uint8_t)nunchuk_data[5] & 1;
button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1;
@ -889,41 +763,72 @@ void readInput(void) {
} }
#endif #endif
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
input1 = (ppm_captured_value[0] - 500) * 2;
input2 = (ppm_captured_value[1] - 500) * 2;
#ifdef SUPPORT_BUTTONS
button1 = ppm_captured_value[5] > 500;
button2 = 0;
#endif
#endif
#if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
input1 = (pwm_captured_ch1_value - 500) * 2;
input2 = (pwm_captured_ch2_value - 500) * 2;
#endif
#ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h
input1 = adc_buffer.l_tx2;
input2 = adc_buffer.l_rx2;
timeoutCnt = 0;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
// Handle received data validity, timeout and fix out-of-sync if necessary // Handle received data validity, timeout and fix out-of-sync if necessary
#ifdef CONTROL_IBUS #ifdef CONTROL_IBUS
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000 ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000
} }
input1 = (ibus_captured_value[0] - 500) * 2; input1.raw = (ibus_captured_value[0] - 500) * 2;
input2 = (ibus_captured_value[1] - 500) * 2; input2.raw = (ibus_captured_value[1] - 500) * 2;
#else #else
input1 = command.steer; input1.raw = command.steer;
input2 = command.speed; input2.raw = command.speed;
#endif #endif
timeoutCnt = 0; timeoutCnt = 0;
#endif #endif
#if defined(SIDEBOARD_SERIAL_USART3)
if (!timeoutFlagSerial_R && Sideboard_R.sensors & SW1_SET) { // If no Timeout and SW1 is set, switch to Sideboard control
input1.raw = Sideboard_R.cmd1;
input2.raw = Sideboard_R.cmd2;
} else {
Sideboard_R.sensors &= ~SW1_SET; // Clear SW1 bit, to switch to default control input
}
#endif
#if defined(SIDEBOARD_SERIAL_USART2) // Priority on the Left sideboard
if (!timeoutFlagSerial_L && Sideboard_L.sensors & SW1_SET) {
input1.raw = Sideboard_L.cmd1;
input2.raw = Sideboard_L.cmd2;
} else {
Sideboard_L.sensors &= ~SW1_SET;
}
#endif
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
input1.raw = (ppm_captured_value[0] - 500) * 2;
input2.raw = (ppm_captured_value[1] - 500) * 2;
#ifdef SUPPORT_BUTTONS
button1 = ppm_captured_value[5] > 500;
button2 = 0;
#endif
#endif
#if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
input1.raw = (pwm_captured_ch1_value - 500) * 2;
input2.raw = (pwm_captured_ch2_value - 500) * 2;
#endif
}
/*
* Add Dead-band to a signal
* This function realizes a dead-band around 0 and scales the input between [out_min, out_max]
*/
void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) {
switch (in->typ){
case 1: // Input is a normal pot
in->cmd = CLAMP(MAP(in->raw, in->min, in->max, 0, out_max), 0, out_max);
case 2: // Input is a mid resting pot
if( in->raw > in->mid - in->deadband && in->raw < in->mid + in->deadband ) {
in->cmd = 0;
} else if(in->raw > in->mid) {
in->cmd = CLAMP(MAP(in->raw, in->mid + in->deadband, in->max, 0, out_max), 0, out_max);
} else {
in->cmd = CLAMP(MAP(in->raw, in->mid - in->deadband, in->min, 0, out_min), out_min, 0);
}
default: // Input is ignored
in->cmd = 0;
}
} }
/* /*
@ -932,17 +837,12 @@ void readInput(void) {
* - MIN/MAX limitations and deadband * - MIN/MAX limitations and deadband
*/ */
void readCommand(void) { void readCommand(void) {
readInput(); readInputRaw();
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
// If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout // If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout
if (IN_RANGE(input1, (int16_t)INPUT1_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT1_MAX_CAL + ADC_PROTECT_THRESH) && if (IN_RANGE(input1.raw, input1.min - ADC_PROTECT_THRESH, input1.max + ADC_PROTECT_THRESH) &&
IN_RANGE(input2, (int16_t)INPUT2_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT2_MAX_CAL + ADC_PROTECT_THRESH)){ IN_RANGE(input2.raw, input2.min - ADC_PROTECT_THRESH, input2.max + ADC_PROTECT_THRESH)) {
if (timeoutFlagADC) { // Check for previous timeout flag
if (timeoutCntADC-- <= 0) // Timeout de-qualification
timeoutFlagADC = 0; // Timeout flag cleared
} else {
timeoutCntADC = 0; // Reset the timeout counter timeoutCntADC = 0; // Reset the timeout counter
}
} else { } else {
if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification
timeoutFlagADC = 1; // Timeout detected timeoutFlagADC = 1; // Timeout detected
@ -970,33 +870,33 @@ void readCommand(void) {
#endif #endif
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
cmd1 = addDeadBand(input1, INPUT1_TYP_CAL, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX); readInputCmd(&input1, INPUT_MIN, INPUT_MAX);
#if !defined(VARIANT_SKATEBOARD) #if !defined(VARIANT_SKATEBOARD)
cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX); readInputCmd(&input2, INPUT_MIN, INPUT_MAX);
#else #else
cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_BRAKE, INPUT_MAX); readInputCmd(&input2, INPUT2_BRAKE, INPUT_MAX);
#endif #endif
#endif #endif
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
#ifdef GAMETRAK_CONNECTION_NORMAL #ifdef GAMETRAK_CONNECTION_NORMAL
cmd1 = adc_buffer.l_rx2; input1.cmd = adc_buffer.l_rx2;
cmd2 = adc_buffer.l_tx2; input2.cmd = adc_buffer.l_tx2;
#endif #endif
#ifdef GAMETRAK_CONNECTION_ALTERNATE #ifdef GAMETRAK_CONNECTION_ALTERNATE
cmd1 = adc_buffer.l_tx2; input1.cmd = adc_buffer.l_tx2;
cmd2 = adc_buffer.l_rx2; inputc.cmd = adc_buffer.l_rx2;
#endif #endif
#endif #endif
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
brakePressed = (uint8_t)(cmd1 > 50); brakePressed = (uint8_t)(input1.cmd > 50);
#endif #endif
if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State
ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way
cmd1 = 0; input1.cmd = 0;
cmd2 = 0; input2.cmd = 0;
} else { } else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request ctrlModReq = ctrlModReqRaw; // Follow the Mode request
} }
@ -1393,6 +1293,114 @@ void sideboardSensors(uint8_t sensors) {
/* =========================== Poweroff Functions =========================== */
/*
* Save Configuration to Flash
* This function makes sure data is not lost after power-off
*/
void saveConfig() {
#ifdef VARIANT_TRANSPOTTER
if (saveValue_valid) {
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0], saveValue);
HAL_FLASH_Lock();
}
#endif
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if (inp_cal_valid || cur_spd_valid) {
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)input1.typ);
EE_WriteVariable(VirtAddVarTab[2] , (uint16_t)input1.min);
EE_WriteVariable(VirtAddVarTab[3] , (uint16_t)input1.mid);
EE_WriteVariable(VirtAddVarTab[4] , (uint16_t)input1.max);
EE_WriteVariable(VirtAddVarTab[5] , (uint16_t)input2.typ);
EE_WriteVariable(VirtAddVarTab[6] , (uint16_t)input2.min);
EE_WriteVariable(VirtAddVarTab[7] , (uint16_t)input2.mid);
EE_WriteVariable(VirtAddVarTab[8] , (uint16_t)input2.max);
EE_WriteVariable(VirtAddVarTab[9] , (uint16_t)rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[10], (uint16_t)rtP_Left.n_max);
HAL_FLASH_Lock();
}
#endif
}
void poweroff(void) {
enable = 0;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("-- Motors disabled --\r\n");
#endif
buzzerCount = 0; // prevent interraction with beep counter
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
saveConfig();
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
while(1) {}
}
void poweroffPressCheck(void) {
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
uint16_t cnt_press = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
HAL_Delay(10);
if (cnt_press++ == 5 * 100) { beepShort(5); }
}
if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec
HAL_Delay(1000);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
beepLong(8);
updateCurSpdLim();
beepShort(5);
} else { // Long press: Calibrate ADC Limits
beepLong(16);
adcCalibLim();
beepShort(5);
}
} else { // Short press: power off
poweroff();
}
}
#elif defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
beepShort(5);
HAL_Delay(300);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
beepLong(5);
HAL_Delay(350);
poweroff();
} else {
setDistance += 0.25;
if (setDistance > 2.6) {
setDistance = 0.5;
}
beepShort(setDistance / 0.25);
saveValue = setDistance * 1000;
saveValue_valid = 1;
}
}
#else
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0; // disable motors
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released
poweroff(); // release power-latch
}
#endif
}
/* =========================== Filtering Functions =========================== */ /* =========================== Filtering Functions =========================== */
/* Low pass filter fixed-point 32 bits: fixdt(1,32,16) /* Low pass filter fixed-point 32 bits: fixdt(1,32,16)