interfisch
/
hoverboard-firmware-hack-foc-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

change steer and speed to speedL and speedR commands

This commit is contained in:
interfisch 2021-02-26 19:29:55 +01:00
parent a4ba5245c6
commit 2bab3aa1a4
1 changed files with 24 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Src/main.c
View File

@ -150,11 +150,15 @@ static uint8_t sideboard_leds_R;
static int16_t speed; // local variable for speed. -1000 to 1000 static int16_t speed; // local variable for speed. -1000 to 1000
#ifndef VARIANT_TRANSPOTTER #ifndef VARIANT_TRANSPOTTER
static int16_t steer; // local variable for steering. -1000 to 1000 //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 steerRateFixdt; // local fixed-point variable for steering rate limiter
static int16_t speedRateFixdt; // local fixed-point variable for speed 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 int16_t speedLeftRateFixdt; // local fixed-point variable for steering rate limiter
static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter 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 #endif
static uint32_t inactivity_timeout_counter; static uint32_t inactivity_timeout_counter;
@ -204,6 +208,8 @@ int main(void) {
int16_t cmdL = 0, cmdR = 0; int16_t cmdL = 0, cmdR = 0;
int16_t cmdL_prev = 0, cmdR_prev = 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 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_adcFilt = adc_buffer.temp;
int16_t board_temp_deg_c; 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)){ 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(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100); beepShort(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters //steerFixdt = speedFixdt = 0; // reset filters
speedLeftFixdt = speedRightFixdt = 0; //reset filters
enable = 1; // enable motors enable = 1; // enable motors
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("-- Motors enabled --\r\n"); printf("-- Motors enabled --\r\n");
@ -277,12 +284,20 @@ int main(void) {
#endif #endif
// ####### LOW-PASS FILTER ####### // ####### LOW-PASS FILTER #######
/*
rateLimiter16(input1[inIdx].cmd , RATE, &steerRateFixdt); rateLimiter16(input1[inIdx].cmd , RATE, &steerRateFixdt);
rateLimiter16(input2[inIdx].cmd , RATE, &speedRateFixdt); rateLimiter16(input2[inIdx].cmd , RATE, &speedRateFixdt);
filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer
speed = (int16_t)(speedFixdt >> 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 ####### // ####### VARIANT_HOVERCAR #######
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
@ -299,7 +314,9 @@ int main(void) {
// ####### MIXER ####### // ####### MIXER #######
// cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX); // 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); // 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) ####### // ####### 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)) { if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {