change steer and speed to speedL and speedR commands
This commit is contained in:
parent
a4ba5245c6
commit
2bab3aa1a4
31
Src/main.c
31
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)) {
|
||||
|
|
Loading…
Reference in New Issue