feedc0de/bobbycar-foc-model
1
0
Fork 0
forked from EFeru/hoverboard-firmware-hack-FOC
Code Pull Requests Activity

Merge pull request #293 from Candas1/master

Browse Source 
Fixes and improvements
This commit is contained in:
Emanuel Feru
2022-05-06 15:34:50 +02:00
committed by GitHub
parent bd4a34dabf 12cc722cef
commit c7aa31b66c
8 changed files with 354 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
.github/ISSUE_TEMPLATE/bug_report.yaml vendored Normal file
View File

@ -0,0 +1,54 @@
name: 🐞 Bug report
description: Create a report to help us improve
labels: "bug"
body:
- type: markdown
attributes:
value: "**Please check the [Wiki](https://github.com/EFeru/hoverboard-firmware-hack-FOC/wiki), it contains tips for troubleshooting**"
- type: dropdown
attributes:
label: Variant
description: What variant of the firmware are you using?
multiple: false
options:
- ADC
- USART
- NUNCHUK
- PPM
- PWM
- IBUS
- HOVERCAR
- TRANSPOTTER
- SKATEBOARD
- HOVERBOARD
validations:
required: true
- type: dropdown
attributes:
label: Control type
description: What Control type are you using?
multiple: false
options:
- Commutation
- Sinusoidale
- FOC
validations:
required: true
- type: dropdown
attributes:
label: Control mode
description: What Control mode are you using?
multiple: false
options:
- Voltage
- Speed
- Torque
validations:
required: true
- type: textarea
attributes:
label: What is the bug and how to reproduce it ?
placeholder: Describe the bug and how to reproduce it
validations:
required: true

9
.github/ISSUE_TEMPLATE/config.yml vendored Normal file
View File

@ -0,0 +1,9 @@
blank_issues_enabled: false
contact_links:
- name: ℹ️ Wiki Pages
url: https://github.com/EFeru/hoverboard-firmware-hack-FOC/wiki
about: Please check the Wiki first
- name: 💬 Telegram Community
url: https://t.me/joinchat/BHWO_RKu2LT5ZxEkvUB8uw
about: Connect with the Telegram Community

60
.github/ISSUE_TEMPLATE/idea---feature-request.yaml vendored Normal file
View File

@ -0,0 +1,60 @@
name: 🚀 Idea / Feature request
description: Suggest an idea for this project
labels: "enhancement"
body:
- type: markdown
attributes:
value: "**Please check the [Wiki](https://github.com/EFeru/hoverboard-firmware-hack-FOC/wiki) for existing features**"
- type: dropdown
attributes:
label: Variant
description: What variant of the firmware are you using?
multiple: false
options:
- ADC
- USART
- NUNCHUK
- PPM
- PWM
- IBUS
- HOVERCAR
- TRANSPOTTER
- SKATEBOARD
- HOVERBOARD
validations:
required: false
- type: dropdown
attributes:
label: Control type
description: What Control type are you using?
multiple: false
options:
- Commutation
- Sinusoidale
- FOC
validations:
required: false
- type: dropdown
attributes:
label: Control mode
description: What Control mode are you using?
multiple: false
options:
- Voltage
- Speed
- Torque
validations:
required: false
- type: textarea
attributes:
label: What can we do to make the firmware better?
placeholder: Consider if code examples or images would help communicate your request
validations:
required: true
- type: textarea
attributes:
label: Describe suggestions or alternatives you have considered
placeholder: A clear and concise description of any alternative solutions or features you've considered
validations:
required: true

54
.github/ISSUE_TEMPLATE/question-about-the-firmware.yaml vendored Normal file
View File

@ -0,0 +1,54 @@
name: ❓ Question about the firmware
description: How to use the firmware to...
labels: "question"
body:
- type: markdown
attributes:
value: "**Please check the [Wiki](https://github.com/EFeru/hoverboard-firmware-hack-FOC/wiki), it might already contain an answer to your question**"
- type: dropdown
attributes:
label: Variant
description: What variant of the firmware are you using?
multiple: false
options:
- ADC
- USART
- NUNCHUK
- PPM
- PWM
- IBUS
- HOVERCAR
- TRANSPOTTER
- SKATEBOARD
- HOVERBOARD
validations:
required: false
- type: dropdown
attributes:
label: Control type
description: What Control type are you using?
multiple: false
options:
- Commutation
- Sinusoidale
- FOC
validations:
required: false
- type: dropdown
attributes:
label: Control mode
description: What Control mode are you using?
multiple: false
options:
- Voltage
- Speed
- Torque
validations:
required: false
- type: textarea
attributes:
label: Description
placeholder: If applicable, indicate what you tried that doesn't work
validations:
required: true

9
Inc/config.h
View File

@ -78,6 +78,7 @@
#define BAT_CELLS 10 // battery number of cells. Normal Hoverboard battery: 10s
#define BAT_LVL2_ENABLE 0 // to beep or not to beep, 1 or 0
#define BAT_LVL1_ENABLE 1 // to beep or not to beep, 1 or 0
#define BAT_DEAD_ENABLE 1 // to poweroff or not to poweroff, 1 or 0
#define BAT_BLINK_INTERVAL 80 // battery led blink interval (80 loops * 5ms ~= 400ms)
#define BAT_LVL5 (390 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Green blink: no beep
#define BAT_LVL4 (380 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Yellow: no beep
@ -299,6 +300,7 @@
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#endif
// #define TANK_STEERING // use for tank steering, each input controls each wheel
// #define ADC_ALTERNATE_CONNECT // use to swap ADC inputs
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
@ -316,7 +318,7 @@
// #define SIDEBOARD_SERIAL_USART3 0
// #define CONTROL_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
// #define DUAL_INPUTS // UART*(Primary) + SIDEBOARD(Auxiliary). Uncomment this to use Dual-inputs
#define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
@ -330,6 +332,7 @@
#define FLASH_WRITE_KEY 0x1002 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h
#endif
// #define TANK_STEERING // use for tank steering, each input controls each wheel
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif
@ -396,6 +399,7 @@
#endif
#define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used.
// #define TANK_STEERING // use for tank steering, each input controls each wheel
// #define SUPPORT_BUTTONS // Define for PPM buttons support
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
// #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
@ -435,6 +439,7 @@
#define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0].
#define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14
#define STEER_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0.
// #define TANK_STEERING // use for tank steering, each input controls each wheel
// #define INVERT_R_DIRECTION
// #define INVERT_L_DIRECTION
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2!
@ -479,6 +484,8 @@
#define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#endif
// #define TANK_STEERING // use for tank steering, each input controls each wheel
#if defined(CONTROL_SERIAL_USART3) && !defined(DUAL_INPUTS)
#define DEBUG_SERIAL_USART2 // left sensor cable debug
#elif defined(DEBUG_SERIAL_USART2) && !defined(DUAL_INPUTS)

4
Src/comms.c
View File

@ -487,7 +487,8 @@ void handle_input(uint8_t *userCommand, uint32_t len)
// If there is already an unprocessed command, exit
if (command.semaphore == 1) return;
if (*userCommand != '$') return; // reject if first character is not $
// Check end of line
userCommand+=len-1; // Go to last char
if (*userCommand != '\n' && *userCommand != '\r'){
@ -495,6 +496,7 @@ void handle_input(uint8_t *userCommand, uint32_t len)
return;
}
userCommand-=len-1; // Come back
userCommand++; // Skip $
int8_t cindex = -1;
int8_t pindex = -1;

56
Src/main.c
View File

@ -216,7 +216,8 @@ int main(void) {
#ifndef VARIANT_TRANSPOTTER
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (input1[inIdx].cmd > -50 && input1[inIdx].cmd < 50) && (input2[inIdx].cmd > -50 && input2[inIdx].cmd < 50)){
if (enable == 0 && !rtY_Left.z_errCode && !rtY_Right.z_errCode &&
ABS(input1[inIdx].cmd) < 50 && ABS(input2[inIdx].cmd) < 50){
beepShort(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters
@ -293,10 +294,15 @@ int main(void) {
}
#endif
// ####### MIXER #######
// 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_MAX);
mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above
#if defined(TANK_STEERING) && !defined(VARIANT_HOVERCAR) && !defined(VARIANT_SKATEBOARD)
// Tank steering (no mixing)
cmdL = steer;
cmdR = speed;
#else
// ####### MIXER #######
mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above
#endif
// ####### SET OUTPUTS (if the target change is less than +/- 100) #######
#ifdef INVERT_R_DIRECTION
@ -407,14 +413,19 @@ int main(void) {
#endif
// ####### SIDEBOARDS HANDLING #######
#if defined(SIDEBOARD_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART2)
sideboardLeds(&sideboard_leds_L);
#if defined(SIDEBOARD_SERIAL_USART2)
sideboardSensors((uint8_t)Sideboard_L.sensors);
#endif
#if defined(SIDEBOARD_SERIAL_USART3) && defined(FEEDBACK_SERIAL_USART3)
sideboardLeds(&sideboard_leds_R);
#if defined(FEEDBACK_SERIAL_USART2)
sideboardLeds(&sideboard_leds_L);
#endif
#if defined(SIDEBOARD_SERIAL_USART3)
sideboardSensors((uint8_t)Sideboard_R.sensors);
#endif
#if defined(FEEDBACK_SERIAL_USART3)
sideboardLeds(&sideboard_leds_R);
#endif
// ####### CALC BOARD TEMPERATURE #######
filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
@ -484,7 +495,15 @@ int main(void) {
poweroffPressCheck();
// ####### BEEP AND EMERGENCY POWEROFF #######
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3
if (TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20){ // poweroff before mainboard burns OR low bat 3
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Powering off, temperature is too high\r\n");
#endif
poweroff();
} else if ( BAT_DEAD_ENABLE && batVoltage < BAT_DEAD && speedAvgAbs < 20){
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Powering off, battery voltage is too low\r\n");
#endif
poweroff();
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
enable = 0;
@ -501,7 +520,7 @@ int main(void) {
beepCount(0, 10, 6);
} else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2
beepCount(0, 10, 30);
} else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1 beep fast (high pitch): Backward spinning motors
} else if (BEEPS_BACKWARD && (((cmdR < -50 || cmdL < -50) && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1 beep fast (high pitch): Backward spinning motors
beepCount(0, 5, 1);
backwardDrive = 1;
} else { // do not beep
@ -510,13 +529,24 @@ int main(void) {
}
inactivity_timeout_counter++;
// ####### INACTIVITY TIMEOUT #######
if (abs(cmdL) > 50 || abs(cmdR) > 50) {
inactivity_timeout_counter = 0;
} else {
inactivity_timeout_counter++;
}
#if defined(CRUISE_CONTROL_SUPPORT) || defined(STANDSTILL_HOLD_ENABLE)
if ((abs(rtP_Left.n_cruiseMotTgt) > 50 && rtP_Left.b_cruiseCtrlEna) ||
(abs(rtP_Right.n_cruiseMotTgt) > 50 && rtP_Right.b_cruiseCtrlEna)) {
inactivity_timeout_counter = 0;
}
#endif
if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Powering off, wheels were inactive for too long\r\n");
#endif
poweroff();
}

196
Src/util.c
View File

@ -303,6 +303,10 @@ void Input_Init(void) {
EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
if (writeCheck == FLASH_WRITE_KEY) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Using the configuration from EEprom\r\n");
#endif
EE_ReadVariable(VirtAddVarTab[1] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[2] , &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal;
for (uint8_t i=0; i<INPUTS_NR; i++) {
@ -314,8 +318,16 @@ void Input_Init(void) {
EE_ReadVariable(VirtAddVarTab[ 8+8*i] , &readVal); input2[i].min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 9+8*i] , &readVal); input2[i].mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[10+8*i] , &readVal); input2[i].max = (int16_t)readVal;
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[i].typ, input1[i].min, input1[i].mid, input1[i].max,
input2[i].typ, input2[i].min, input2[i].mid, input2[i].max);
}
} else {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Using the configuration from config.h\r\n");
#endif
for (uint8_t i=0; i<INPUTS_NR; i++) {
if (input1[i].typDef == 3) { // If Input type defined is 3 (auto), identify the input type based on the values from config.h
input1[i].typ = checkInputType(input1[i].min, input1[i].mid, input1[i].max);
@ -327,6 +339,9 @@ void Input_Init(void) {
} else {
input2[i].typ = input2[i].typDef;
}
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[i].typ, input1[i].min, input1[i].mid, input1[i].max,
input2[i].typ, input2[i].min, input2[i].mid, input2[i].max);
}
}
HAL_FLASH_Lock();
@ -444,14 +459,25 @@ void beepShortMany(uint8_t cnt, int8_t dir) {
void calcAvgSpeed(void) {
// Calculate measured average speed. The minus sign (-) is because motors spin in opposite directions
#if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION)
speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2;
#elif !defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION)
speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot) / 2;
#elif defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION)
speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot) / 2;
#elif defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION)
speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot) / 2;
speedAvg = 0;
#if defined(MOTOR_LEFT_ENA)
#if defined(INVERT_L_DIRECTION)
speedAvg -= rtY_Left.n_mot;
#else
speedAvg += rtY_Left.n_mot;
#endif
#endif
#if defined(MOTOR_RIGHT_ENA)
#if defined(INVERT_R_DIRECTION)
speedAvg += rtY_Right.n_mot;
#else
speedAvg -= rtY_Right.n_mot;
#endif
// Average only if both motors are enabled
#if defined(MOTOR_LEFT_ENA)
speedAvg /= 2;
#endif
#endif
// Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1)
@ -521,16 +547,13 @@ void adcCalibLim(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input1 is ");
#endif
input1[inIdx].typ = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
if (input1[inIdx].typ == input1[inIdx].typDef || input1[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input1[inIdx].min = INPUT1_MIN_temp + input_margin;
input1[inIdx].mid = INPUT1_MID_temp;
input1[inIdx].max = INPUT1_MAX_temp - input_margin;
uint8_t input1TypTemp = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
if (input1TypTemp == input1[inIdx].typDef || input1[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
input1[inIdx].typ = 0; // Disable input
input1TypTemp = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
@ -539,26 +562,42 @@ void adcCalibLim(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input2 is ");
#endif
input2[inIdx].typ = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (input2[inIdx].typ == input2[inIdx].typDef || input2[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input2[inIdx].min = INPUT2_MIN_temp + input_margin;
input2[inIdx].mid = INPUT2_MID_temp;
input2[inIdx].max = INPUT2_MAX_temp - input_margin;
uint8_t input2TypTemp = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (input2TypTemp == input2[inIdx].typDef || input2[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
input2[inIdx].typ = 0; // Disable input
input2TypTemp = 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[inIdx].typ, input1[inIdx].min, input1[inIdx].mid, input1[inIdx].max,
input2[inIdx].typ, input2[inIdx].min, input2[inIdx].mid, input2[inIdx].max);
#endif
// At least one of the inputs is not ignored
if (input1TypTemp != 0 || input2TypTemp != 0){
input1[inIdx].typ = input1TypTemp;
input1[inIdx].min = INPUT1_MIN_temp + input_margin;
input1[inIdx].mid = INPUT1_MID_temp;
input1[inIdx].max = INPUT1_MAX_temp - input_margin;
input2[inIdx].typ = input2TypTemp;
input2[inIdx].min = INPUT2_MIN_temp + input_margin;
input2[inIdx].mid = INPUT2_MID_temp;
input2[inIdx].max = INPUT2_MAX_temp - input_margin;
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[inIdx].typ, input1[inIdx].min, input1[inIdx].mid, input1[inIdx].max,
input2[inIdx].typ, input2[inIdx].min, input2[inIdx].mid, input2[inIdx].max);
#endif
}else{
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Both inputs cannot be ignored, calibration rejected.\r\n");
#endif
}
#endif
#endif // AUTO_CALIBRATION_ENA
@ -1040,7 +1079,7 @@ void readCommand(void) {
#endif
#if defined(CRUISE_CONTROL_SUPPORT) && (defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT))
cruiseControl(button1); // Cruise control activation/deactivation
cruiseControl(button1); // Cruise control activation/deactivation
#endif
}
@ -1058,16 +1097,16 @@ void usart2_rx_check(void)
#endif
#if defined(DEBUG_SERIAL_USART2)
uint8_t ptr[SERIAL_BUFFER_SIZE];
uint8_t ptr_debug[SERIAL_BUFFER_SIZE];
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
memcpy(&ptr[0], &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer
memcpy(&ptr_debug[0], &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
memcpy(&ptr[rx_buffer_L_len - old_pos], &rx_buffer_L[0], pos); // Copy remaining data
memcpy(&ptr_debug[rx_buffer_L_len - old_pos], &rx_buffer_L[0], pos); // Copy remaining data
}
usart_process_debug(ptr, rx_buffer_L_len - old_pos + pos); // Process data
usart_process_debug(ptr_debug, rx_buffer_L_len - old_pos + pos); // Process data
}
}
#endif // DEBUG_SERIAL_USART2
@ -1130,17 +1169,17 @@ void usart3_rx_check(void)
#endif
#if defined(DEBUG_SERIAL_USART3)
uint8_t ptr[SERIAL_BUFFER_SIZE];
uint8_t ptr_debug[SERIAL_BUFFER_SIZE];
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
memcpy(&ptr[0], &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer
memcpy(&ptr_debug[0], &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
memcpy(&ptr[rx_buffer_R_len - old_pos], &rx_buffer_R[0], pos); // Copy remaining data
memcpy(&ptr_debug[rx_buffer_R_len - old_pos], &rx_buffer_R[0], pos); // Copy remaining data
}
usart_process_debug(ptr, rx_buffer_R_len - old_pos + pos); // Process data
usart_process_debug(ptr_debug, rx_buffer_R_len - old_pos + pos); // Process data
}
}
#endif // DEBUG_SERIAL_USART3
@ -1387,9 +1426,7 @@ void sideboardSensors(uint8_t sensors) {
sensor1_prev = sensor1_index;
} else { // Use Optical switches
sensor1_trig = (sensors & SENSOR1_SET) && !sensor1_prev; // rising edge detection
sensor2_trig = (sensors & SENSOR2_SET) && !sensor2_prev; // rising edge detection
sensor1_prev = sensors & SENSOR1_SET;
sensor2_prev = sensors & SENSOR2_SET;
}
// Control MODE and Control Type Handling
@ -1418,11 +1455,7 @@ void sideboardSensors(uint8_t sensors) {
if (++sensor1_index > 4) { sensor1_index = 0; }
}
#ifdef CRUISE_CONTROL_SUPPORT // Cruise Control Activation/Deactivation
if (sensor2_trig) {
cruiseControl(sensor2_trig);
}
#else // Field Weakening Activation/Deactivation
// Field Weakening Activation/Deactivation
static uint8_t sensor2_index = 1; // holds the press index number for sensor2, when used as a button
// Override in case the Sideboard control is Active
@ -1433,25 +1466,33 @@ void sideboardSensors(uint8_t sensors) {
sensor2_trig = 1;
}
sensor2_prev = sensor2_index;
}else{
sensor2_trig = (sensors & SENSOR2_SET) && !sensor2_prev; // rising edge detection
sensor2_prev = sensors & SENSOR2_SET;
}
if (sensor2_trig) {
switch (sensor2_index) {
case 0: // FW Disabled
rtP_Left.b_fieldWeakEna = 0;
rtP_Right.b_fieldWeakEna = 0;
Input_Lim_Init();
break;
case 1: // FW Enabled
rtP_Left.b_fieldWeakEna = 1;
rtP_Right.b_fieldWeakEna = 1;
Input_Lim_Init();
break;
#ifdef CRUISE_CONTROL_SUPPORT // Cruise Control Activation/Deactivation
if (sensor2_trig) {
cruiseControl(sensor2_trig);
}
if (inIdx == inIdx_prev) { beepShortMany(sensor2_index + 1, 1); }
if (++sensor2_index > 1) { sensor2_index = 0; }
}
#endif // CRUISE_CONTROL_SUPPORT
#else
if (sensor2_trig) {
switch (sensor2_index) {
case 0: // FW Disabled
rtP_Left.b_fieldWeakEna = 0;
rtP_Right.b_fieldWeakEna = 0;
Input_Lim_Init();
break;
case 1: // FW Enabled
rtP_Left.b_fieldWeakEna = 1;
rtP_Right.b_fieldWeakEna = 1;
Input_Lim_Init();
break;
}
if (inIdx == inIdx_prev) { beepShortMany(sensor2_index + 1, 1); }
if (++sensor2_index > 1) { sensor2_index = 0; }
}
#endif // CRUISE_CONTROL_SUPPORT
#endif
}
@ -1473,6 +1514,10 @@ void saveConfig() {
#endif
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if (inp_cal_valid || cur_spd_valid) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Saving configuration to EEprom\r\n");
#endif
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)rtP_Left.i_max);
@ -1513,12 +1558,14 @@ void poweroff(void) {
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 > 8) enable = 0;
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
@ -1534,7 +1581,10 @@ void poweroffPressCheck(void) {
#endif
}
} else if (cnt_press > 8) { // Short press: power off (80 ms debounce)
poweroff();
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Powering off, button has been pressed\r\n");
#endif
poweroff();
}
}
#elif defined(VARIANT_TRANSPOTTER)
@ -1634,22 +1684,22 @@ void rateLimiter16(int16_t u, int16_t rate, int16_t *y) {
* Parameters: SPEED_COEFFICIENT, STEER_COEFFICIENT = fixdt(0,16,14)
*/
void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t *rty_speedL) {
int16_t prodSpeed;
int16_t prodSteer;
int32_t tmp;
int16_t prodSpeed;
int16_t prodSteer;
int32_t tmp;
prodSpeed = (int16_t)((rtu_speed * (int16_t)SPEED_COEFFICIENT) >> 14);
prodSteer = (int16_t)((rtu_steer * (int16_t)STEER_COEFFICIENT) >> 14);
prodSpeed = (int16_t)((rtu_speed * (int16_t)SPEED_COEFFICIENT) >> 14);
prodSteer = (int16_t)((rtu_steer * (int16_t)STEER_COEFFICIENT) >> 14);
tmp = prodSpeed - prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedR = CLAMP(*rty_speedR, INPUT_MIN, INPUT_MAX);
tmp = prodSpeed - prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedR = CLAMP(*rty_speedR, INPUT_MIN, INPUT_MAX);
tmp = prodSpeed + prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedL = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedL = CLAMP(*rty_speedL, INPUT_MIN, INPUT_MAX);
tmp = prodSpeed + prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedL = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedL = CLAMP(*rty_speedL, INPUT_MIN, INPUT_MAX);
}