diff --git a/Inc/config.h b/Inc/config.h index 9c20a66..cbd43e5 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -402,6 +402,7 @@ // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // Extra functionality + // #define CRUISE_CONTROL_SUPPORT // [-] Flag to enable Cruise Control support. Activation/Deactivation is done by sideboard button or Brake pedal press. // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. // #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode. // #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released). diff --git a/Inc/util.h b/Inc/util.h index 7e7c83d..5cba7d2 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -72,6 +72,7 @@ void saveConfig(void); int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); void standstillHold(int16_t *speedCmd); void electricBrake(uint16_t speedBlend, uint8_t reverseDir); +void cruiseControl(uint8_t button); // Poweroff Functions void poweroff(void); diff --git a/Src/main.c b/Src/main.c index c4b7f1b..da3f99a 100644 --- a/Src/main.c +++ b/Src/main.c @@ -224,7 +224,8 @@ int main(void) { } if (cmd1 > 30) { // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving - cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); + cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); + cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna); // Cruise control deactivated by Brake pedal if it was active } #endif diff --git a/Src/util.c b/Src/util.c index 8ec88cd..6de9c31 100644 --- a/Src/util.c +++ b/Src/util.c @@ -186,13 +186,6 @@ static uint32_t command_len = sizeof(command); #endif #endif -#if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3)) -static uint8_t sensor1_prev; // holds the previous sensor1 state -static uint8_t sensor2_prev; // holds the previous sensor2 state -static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button -static uint8_t sensor2_index; // holds the press index number for sensor2, when used as a button -#endif - #if defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) static uint8_t button1; // Blue static uint8_t button2; // Green @@ -390,12 +383,12 @@ void shortBeep(uint8_t freq) { void shortBeepMany(uint8_t cnt, int8_t dir) { if (dir >= 0) { // increasing tone - for(uint8_t i = cnt; i > 0; i--) { - shortBeep(i + 2); + for(uint8_t i = 2*cnt; i >= 2; i=i-2) { + shortBeep(i + 3); } } else { // decreasing tone - for(uint8_t i = 0; i < cnt; i++) { - shortBeep(i + 2); + for(uint8_t i = 2; i <= 2*cnt; i=i+2) { + shortBeep(i + 3); } } } @@ -648,6 +641,30 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) { #endif } + /* + * Cruise Control Function + * This function activates/deactivates cruise control. + * + * Input: button (as a pulse) + * Output: none + */ +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; + shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also. + } else if (button && rtP_Left.b_cruiseCtrlEna) { // Cruise control deactivated + rtP_Left.b_cruiseCtrlEna = 0; + rtP_Right.b_cruiseCtrlEna = 0; + shortBeepMany(2, -1); + } + #endif +} + + /* =========================== Poweroff Functions =========================== */ @@ -869,17 +886,7 @@ void readCommand(void) { } #if defined(CRUISE_CONTROL_SUPPORT) && (defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)) - if (button1 && !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; - shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also. - } else if (button1 && rtP_Left.b_cruiseCtrlEna) { // Cruise control deactivated - rtP_Left.b_cruiseCtrlEna = 0; - rtP_Right.b_cruiseCtrlEna = 0; - shortBeepMany(2, -1); - } + cruiseControl(button1); // Cruise control activation/deactivation #endif } @@ -1199,6 +1206,7 @@ void sideboardLeds(uint8_t *leds) { */ void sideboardSensors(uint8_t sensors) { #if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3)) + static uint8_t sensor1_prev, sensor2_prev; uint8_t sensor1_rising_edge, sensor2_rising_edge; sensor1_rising_edge = (sensors & SENSOR1_SET) && !sensor1_prev; sensor2_rising_edge = (sensors & SENSOR2_SET) && !sensor2_prev; @@ -1206,6 +1214,7 @@ void sideboardSensors(uint8_t sensors) { sensor2_prev = sensors & SENSOR2_SET; // Control MODE and Control Type Handling: use Sensor1 as push button + static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button if (sensor1_rising_edge) { sensor1_index++; if (sensor1_index > 4) { sensor1_index = 0; } @@ -1234,23 +1243,30 @@ void sideboardSensors(uint8_t sensors) { } // Field Weakening: use Sensor2 as push button - if (sensor2_rising_edge) { - sensor2_index++; - if (sensor2_index > 1) { sensor2_index = 0; } - 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 + if (sensor2_rising_edge) { + cruiseControl(sensor2_rising_edge); } - shortBeepMany(sensor2_index + 1, 1); - } + #else + static uint8_t sensor2_index; // holds the press index number for sensor2, when used as a button + if (sensor2_rising_edge) { + sensor2_index++; + if (sensor2_index > 1) { sensor2_index = 0; } + 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; + } + shortBeepMany(sensor2_index + 1, 1); + } + #endif // CRUISE_CONTROL_SUPPORT #endif }