From 873fc8ea14ea2e4049b6f0241736382d712ee867 Mon Sep 17 00:00:00 2001 From: Guruth <1669916+Guruth@users.noreply.github.com> Date: Sun, 22 May 2022 00:02:07 +0200 Subject: [PATCH 1/6] Added Multi Mode Drive (cherry picked from commit 0167a1d81cf0eaaebc7196e2fcf7120a13438f4d) --- Inc/config.h | 13 +++++++++++ Src/main.c | 61 ++++++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 70 insertions(+), 4 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index f78bd19..2bf30c8 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -524,6 +524,19 @@ // #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). // #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging. + + #define MULTI_MODE_DRIVE + #ifdef MULTI_MODE_DRIVE + #define MULTI_MODE_DRIVE_M1_MAX 175 + #define MULTI_MODE_DRIVE_M1_RATE 175 + + #define MULTI_MODE_DRIVE_M2_MAX 500 + #define MULTI_MODE_DRIVE_M2_RATE 300 + + #define MULTI_MODE_DRIVE_M3_MAX 1000 + #define MULTI_MODE_DRIVE_M3_RATE 450 + #endif + #endif // Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses) diff --git a/Src/main.c b/Src/main.c index ce2b142..e531a64 100644 --- a/Src/main.c +++ b/Src/main.c @@ -162,6 +162,11 @@ static uint32_t buzzerTimer_prev = 0; static uint32_t inactivity_timeout_counter; static MultipleTap MultipleTapBrake; // define multiple tap functionality for the Brake pedal +static uint16_t rate = RATE; +#ifdef MULTI_MODE_DRIVE + static uint16_t max_speed; + static uint8_t drive_mode; +#endif int main(void) { HAL_Init(); @@ -205,9 +210,29 @@ int main(void) { 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; + #ifdef MULTI_MODE_DRIVE + int16_t adc_one = adc_buffer.l_rx2; + int16_t adc_two = adc_buffer.l_tx2; + + if(adc_one >= input1[0].min){ + drive_mode = 0; + } else if(adc_two >= input2[0].min){ + drive_mode = 2; + } else{ + drive_mode = 1; + } + + printf("Drive mode %i selected \r\n", drive_mode); + #endif + // Loop until button is released while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + #ifdef MULTI_MODE_DRIVE + // Wait until triggers are released + while((adc_buffer.l_rx2 + adc_buffer.l_tx2) <= (input1[0].min + input2[0].min)) { HAL_Delay(10); } + #endif + while(1) { if (buzzerTimer - buzzerTimer_prev > 16*DELAY_IN_MAIN_LOOP) { // 1 ms = 16 ticks buzzerTimer @@ -274,9 +299,20 @@ int main(void) { } #endif + + #ifdef MULTI_MODE_DRIVE + if(drive_mode == 0){ + rate = MULTI_MODE_DRIVE_M1_RATE; + } else if( drive_mode == 1){ + rate = MULTI_MODE_DRIVE_M2_RATE; + } else if(drive_mode == 2){ + rate = MULTI_MODE_DRIVE_M3_RATE; + } + #endif + // ####### LOW-PASS FILTER ####### - rateLimiter16(input1[inIdx].cmd , RATE, &steerRateFixdt); - rateLimiter16(input2[inIdx].cmd , RATE, &speedRateFixdt); + rateLimiter16(input1[inIdx].cmd , rate, &steerRateFixdt); + rateLimiter16(input2[inIdx].cmd , rate, &speedRateFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer @@ -285,6 +321,22 @@ int main(void) { // ####### VARIANT_HOVERCAR ####### #ifdef VARIANT_HOVERCAR if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input) + + #ifdef MULTI_MODE_DRIVE + + if(drive_mode == 0){ + max_speed = MULTI_MODE_DRIVE_M1_MAX; + } else if( drive_mode == 1){ + max_speed = MULTI_MODE_DRIVE_M2_MAX; + } else if(drive_mode == 2){ + max_speed = MULTI_MODE_DRIVE_M3_MAX; + } + + if(speed >= max_speed){ + speed = max_speed; + } + #endif + if (!MultipleTapBrake.b_multipleTap) { // Check driving direction speed = steer + speed; // Forward driving: in this case steer = Brake, speed = Throttle } else { @@ -446,7 +498,7 @@ int main(void) { #if defined(DEBUG_SERIAL_PROTOCOL) process_debug(); #else - 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 Speed:%i\r\n", input1[inIdx].raw, // 1: INPUT1 input2[inIdx].raw, // 2: INPUT2 cmdL, // 3: output command: [-1000, 1000] @@ -454,7 +506,8 @@ int main(void) { adc_buffer.batt1, // 5: for battery voltage calibration batVoltageCalib, // 6: for verifying battery voltage calibration board_temp_adcFilt, // 7: for board temperature calibration - board_temp_deg_c); // 8: for verifying board temperature calibration + board_temp_deg_c, + speed); // 8: for verifying board temperature calibration #endif } #endif From 4ef368330d57fa1902c8246142b62a10cacd782f Mon Sep 17 00:00:00 2001 From: Guruth <1669916+Guruth@users.noreply.github.com> Date: Sun, 22 May 2022 11:40:08 +0200 Subject: [PATCH 2/6] Fixed Trigger released check for MultiModeDrive (cherry picked from commit 2a2e2a2091494e8257b5e558077ca7a3c2ac575f) --- Src/main.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Src/main.c b/Src/main.c index e531a64..5a09b21 100644 --- a/Src/main.c +++ b/Src/main.c @@ -162,11 +162,14 @@ static uint32_t buzzerTimer_prev = 0; static uint32_t inactivity_timeout_counter; static MultipleTap MultipleTapBrake; // define multiple tap functionality for the Brake pedal -static uint16_t rate = RATE; +static uint16_t rate = RATE; // Adjustable rate to support multiple drive modes on startup + #ifdef MULTI_MODE_DRIVE - static uint16_t max_speed; static uint8_t drive_mode; + static uint16_t max_speed; #endif + + int main(void) { HAL_Init(); @@ -230,7 +233,7 @@ int main(void) { #ifdef MULTI_MODE_DRIVE // Wait until triggers are released - while((adc_buffer.l_rx2 + adc_buffer.l_tx2) <= (input1[0].min + input2[0].min)) { HAL_Delay(10); } + while((adc_buffer.l_rx2 + adc_buffer.l_tx2) >= (input1[0].min + input2[0].min)) { HAL_Delay(10); } #endif while(1) { From 6b2cf03ef1458d64881dd972b12b5749c440c6f9 Mon Sep 17 00:00:00 2001 From: Guruth <1669916+Guruth@users.noreply.github.com> Date: Sun, 22 May 2022 11:48:03 +0200 Subject: [PATCH 3/6] Disabled MULTI_MODE_DRIVE by default --- Inc/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Inc/config.h b/Inc/config.h index 2bf30c8..1631003 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -525,7 +525,7 @@ // #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released). // #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging. - #define MULTI_MODE_DRIVE + // #define MULTI_MODE_DRIVE #ifdef MULTI_MODE_DRIVE #define MULTI_MODE_DRIVE_M1_MAX 175 #define MULTI_MODE_DRIVE_M1_RATE 175 From 87f9d4e8a397b6c9bafa4551172e19ee8d2bd048 Mon Sep 17 00:00:00 2001 From: Guruth <1669916+Guruth@users.noreply.github.com> Date: Sun, 22 May 2022 20:36:27 +0200 Subject: [PATCH 4/6] Moved rate, max_speed initialization out of the drive loop --- Src/main.c | 50 +++++++++++++++++++------------------------------- 1 file changed, 19 insertions(+), 31 deletions(-) diff --git a/Src/main.c b/Src/main.c index 5a09b21..4cd610c 100644 --- a/Src/main.c +++ b/Src/main.c @@ -214,18 +214,26 @@ int main(void) { int16_t board_temp_adcFilt = adc_buffer.temp; #ifdef MULTI_MODE_DRIVE - int16_t adc_one = adc_buffer.l_rx2; - int16_t adc_two = adc_buffer.l_tx2; + if(adc_buffer.l_rx2 >= input1[0].min){ + drive_mode = 0; + max_speed = MULTI_MODE_DRIVE_M1_MAX; + rate = MULTI_MODE_DRIVE_M1_RATE; + } else if(adc_buffer.l_tx2 >= input2[0].min){ + drive_mode = 2; + max_speed = MULTI_MODE_DRIVE_M3_MAX; + rate = MULTI_MODE_DRIVE_M3_RATE; + } else{ + drive_mode = 1; + max_speed = MULTI_MODE_DRIVE_M2_MAX; + rate = MULTI_MODE_DRIVE_M2_RATE; + } - if(adc_one >= input1[0].min){ - drive_mode = 0; - } else if(adc_two >= input2[0].min){ - drive_mode = 2; - } else{ - drive_mode = 1; - } - - printf("Drive mode %i selected \r\n", drive_mode); + printf( + "Drive mode %i selected: max_speed:%i acc_rate:%i \r\n", + drive_mode, + max_speed, + rate + ); #endif // Loop until button is released @@ -302,17 +310,6 @@ int main(void) { } #endif - - #ifdef MULTI_MODE_DRIVE - if(drive_mode == 0){ - rate = MULTI_MODE_DRIVE_M1_RATE; - } else if( drive_mode == 1){ - rate = MULTI_MODE_DRIVE_M2_RATE; - } else if(drive_mode == 2){ - rate = MULTI_MODE_DRIVE_M3_RATE; - } - #endif - // ####### LOW-PASS FILTER ####### rateLimiter16(input1[inIdx].cmd , rate, &steerRateFixdt); rateLimiter16(input2[inIdx].cmd , rate, &speedRateFixdt); @@ -326,15 +323,6 @@ int main(void) { if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input) #ifdef MULTI_MODE_DRIVE - - if(drive_mode == 0){ - max_speed = MULTI_MODE_DRIVE_M1_MAX; - } else if( drive_mode == 1){ - max_speed = MULTI_MODE_DRIVE_M2_MAX; - } else if(drive_mode == 2){ - max_speed = MULTI_MODE_DRIVE_M3_MAX; - } - if(speed >= max_speed){ speed = max_speed; } From cb7f3e91839885095184de43ee8b0c69abd3b8dd Mon Sep 17 00:00:00 2001 From: Guruth <1669916+Guruth@users.noreply.github.com> Date: Sun, 22 May 2022 20:41:21 +0200 Subject: [PATCH 5/6] Removed speed from logging --- Src/main.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Src/main.c b/Src/main.c index 4cd610c..6b2274e 100644 --- a/Src/main.c +++ b/Src/main.c @@ -489,7 +489,7 @@ int main(void) { #if defined(DEBUG_SERIAL_PROTOCOL) process_debug(); #else - printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i Speed:%i\r\n", + printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i \r\n", input1[inIdx].raw, // 1: INPUT1 input2[inIdx].raw, // 2: INPUT2 cmdL, // 3: output command: [-1000, 1000] @@ -497,8 +497,7 @@ int main(void) { adc_buffer.batt1, // 5: for battery voltage calibration batVoltageCalib, // 6: for verifying battery voltage calibration board_temp_adcFilt, // 7: for board temperature calibration - board_temp_deg_c, - speed); // 8: for verifying board temperature calibration + board_temp_deg_c); // 8: for verifying board temperature calibration #endif } #endif From 5505a712602b0e0625be4be933d2fc59cc06e6d1 Mon Sep 17 00:00:00 2001 From: EFeru Date: Sun, 5 Jun 2022 14:50:29 +0200 Subject: [PATCH 6/6] Minor styling --- Inc/config.h | 12 ++++++------ Src/main.c | 23 +++++++++-------------- 2 files changed, 15 insertions(+), 20 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index 1631003..3b839d2 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -527,14 +527,14 @@ // #define MULTI_MODE_DRIVE #ifdef MULTI_MODE_DRIVE - #define MULTI_MODE_DRIVE_M1_MAX 175 - #define MULTI_MODE_DRIVE_M1_RATE 175 + #define MULTI_MODE_DRIVE_M1_MAX 175 + #define MULTI_MODE_DRIVE_M1_RATE 175 - #define MULTI_MODE_DRIVE_M2_MAX 500 - #define MULTI_MODE_DRIVE_M2_RATE 300 + #define MULTI_MODE_DRIVE_M2_MAX 500 + #define MULTI_MODE_DRIVE_M2_RATE 300 - #define MULTI_MODE_DRIVE_M3_MAX 1000 - #define MULTI_MODE_DRIVE_M3_RATE 450 + #define MULTI_MODE_DRIVE_M3_MAX 1000 + #define MULTI_MODE_DRIVE_M3_RATE 450 #endif #endif diff --git a/Src/main.c b/Src/main.c index 6b2274e..ccfa2ab 100644 --- a/Src/main.c +++ b/Src/main.c @@ -214,26 +214,21 @@ int main(void) { int16_t board_temp_adcFilt = adc_buffer.temp; #ifdef MULTI_MODE_DRIVE - if(adc_buffer.l_rx2 >= input1[0].min){ + if (adc_buffer.l_rx2 >= input1[0].min) { drive_mode = 0; max_speed = MULTI_MODE_DRIVE_M1_MAX; rate = MULTI_MODE_DRIVE_M1_RATE; - } else if(adc_buffer.l_tx2 >= input2[0].min){ + } else if (adc_buffer.l_tx2 >= input2[0].min) { drive_mode = 2; max_speed = MULTI_MODE_DRIVE_M3_MAX; rate = MULTI_MODE_DRIVE_M3_RATE; - } else{ + } else { drive_mode = 1; max_speed = MULTI_MODE_DRIVE_M2_MAX; rate = MULTI_MODE_DRIVE_M2_RATE; } - printf( - "Drive mode %i selected: max_speed:%i acc_rate:%i \r\n", - drive_mode, - max_speed, - rate - ); + printf("Drive mode %i selected: max_speed:%i acc_rate:%i \r\n", drive_mode, max_speed, rate); #endif // Loop until button is released @@ -311,8 +306,8 @@ int main(void) { #endif // ####### LOW-PASS FILTER ####### - rateLimiter16(input1[inIdx].cmd , rate, &steerRateFixdt); - rateLimiter16(input2[inIdx].cmd , rate, &speedRateFixdt); + rateLimiter16(input1[inIdx].cmd, rate, &steerRateFixdt); + rateLimiter16(input2[inIdx].cmd, rate, &speedRateFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer @@ -323,9 +318,9 @@ int main(void) { if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input) #ifdef MULTI_MODE_DRIVE - if(speed >= max_speed){ - speed = max_speed; - } + if (speed >= max_speed) { + speed = max_speed; + } #endif if (!MultipleTapBrake.b_multipleTap) { // Check driving direction