diff --git a/.github/ISSUE_TEMPLATE/bug_report.yaml b/.github/ISSUE_TEMPLATE/bug_report.yaml new file mode 100644 index 0000000..038b635 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.yaml @@ -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 diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000..7acde52 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -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 diff --git a/.github/ISSUE_TEMPLATE/idea---feature-request.yaml b/.github/ISSUE_TEMPLATE/idea---feature-request.yaml new file mode 100644 index 0000000..fa24909 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/idea---feature-request.yaml @@ -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 diff --git a/.github/ISSUE_TEMPLATE/question-about-the-firmware.yaml b/.github/ISSUE_TEMPLATE/question-about-the-firmware.yaml new file mode 100644 index 0000000..0aa55cd --- /dev/null +++ b/.github/ISSUE_TEMPLATE/question-about-the-firmware.yaml @@ -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 diff --git a/Inc/config.h b/Inc/config.h index 65bb8f8..f78bd19 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -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) diff --git a/Src/comms.c b/Src/comms.c index 835aded..71aa770 100644 --- a/Src/comms.c +++ b/Src/comms.c @@ -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; diff --git a/Src/main.c b/Src/main.c index 2f665da..ce2b142 100644 --- a/Src/main.c +++ b/Src/main.c @@ -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(); } diff --git a/Src/util.c b/Src/util.c index 7403cd4..cf46bff 100644 --- a/Src/util.c +++ b/Src/util.c @@ -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 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); }