From 2bab3aa1a46d1754ecc81692524120122b5b07f7 Mon Sep 17 00:00:00 2001 From: Fisch Date: Fri, 26 Feb 2021 19:29:55 +0100 Subject: [PATCH] change steer and speed to speedL and speedR commands --- Src/main.c | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/Src/main.c b/Src/main.c index a8b370f..9571c0b 100644 --- a/Src/main.c +++ b/Src/main.c @@ -150,11 +150,15 @@ static uint8_t sideboard_leds_R; static int16_t speed; // local variable for speed. -1000 to 1000 #ifndef VARIANT_TRANSPOTTER - static int16_t steer; // local variable for steering. -1000 to 1000 - static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter - static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter - static int32_t steerFixdt; // local fixed-point variable for steering low-pass filter - static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter + //static int16_t steer; // local variable for steering. -1000 to 1000 + //static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter + //static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter + static int16_t speedLeftRateFixdt; // local fixed-point variable for steering rate limiter + static int16_t speedRightRateFixdt; // local fixed-point variable for speed rate limiter + //static int32_t steerFixdt; // local fixed-point variable for steering low-pass filter + //static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter + static int16_t speedLeftFixdt; // local fixed-point variable for speedLeft low-pass filter + static int16_t speedRightFixdt; // local fixed-point variable for speedRight low-pass filter #endif static uint32_t inactivity_timeout_counter; @@ -204,6 +208,8 @@ int main(void) { int16_t cmdL = 0, cmdR = 0; int16_t cmdL_prev = 0, cmdR_prev = 0; + int16_t speedL = 0, speedR = 0; + 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_deg_c; @@ -222,7 +228,8 @@ int main(void) { 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)){ beepShort(6); // make 2 beeps indicating the motor enable beepShort(4); HAL_Delay(100); - steerFixdt = speedFixdt = 0; // reset filters + //steerFixdt = speedFixdt = 0; // reset filters + speedLeftFixdt = speedRightFixdt = 0; //reset filters enable = 1; // enable motors #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("-- Motors enabled --\r\n"); @@ -277,12 +284,20 @@ int main(void) { #endif // ####### LOW-PASS FILTER ####### + /* 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 speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer + */ + rateLimiter16(input1[inIdx].cmd , RATE, &speedLeftRateFixdt); + rateLimiter16(input2[inIdx].cmd , RATE, &speedRightRateFixdt); + filtLowPass32(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt); + filtLowPass32(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt); + speedL = (int16_t)(speedLeftFixdt >> 16); // convert fixed-point to integer + speedR = (int16_t)(speedRightRateFixdt >> 16); // convert fixed-point to integer // ####### VARIANT_HOVERCAR ####### #ifdef VARIANT_HOVERCAR @@ -299,7 +314,9 @@ int main(void) { // ####### 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 + //mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above + cmdL = speedL; //straight copy, no mixing needed + cmdR = speedR; //straight copy // ####### SET OUTPUTS (if the target change is less than +/- 100) ####### if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {