From 5612af0a90531012d3404353fb0fc9e1b16eb987 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Mon, 25 Apr 2022 21:11:55 +0200 Subject: [PATCH] Fix average speed --- Inc/config.h | 2 +- Src/comms.c | 6 +++--- Src/util.c | 29 +++++++++++++++++++++++++---- 3 files changed, 29 insertions(+), 8 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index 483fcb7..f78bd19 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -318,7 +318,7 @@ // #define SIDEBOARD_SERIAL_USART3 0 // #define CONTROL_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino // #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! - + // #define DUAL_INPUTS // UART*(Primary) + SIDEBOARD(Auxiliary). Uncomment this to use Dual-inputs #define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section diff --git a/Src/comms.c b/Src/comms.c index 90219bb..73eb93d 100644 --- a/Src/comms.c +++ b/Src/comms.c @@ -487,9 +487,8 @@ void handle_input(uint8_t *userCommand, uint32_t len) // If there is already an unprocessed command, exit if (command.semaphore == 1) return; - - if (*userCommand > 127) return; // reject if first character is not ascii - + if (*userCommand != '$') return; // reject if first character is not # + // Check end of line userCommand+=len-1; // Go to last char if (*userCommand != '\n' && *userCommand != '\r'){ @@ -497,6 +496,7 @@ void handle_input(uint8_t *userCommand, uint32_t len) return; } userCommand-=len-1; // Come back + userCommand++; // Skip # int8_t cindex = -1; int8_t pindex = -1; diff --git a/Src/util.c b/Src/util.c index 4ee899f..419acfc 100644 --- a/Src/util.c +++ b/Src/util.c @@ -460,13 +460,34 @@ void beepShortMany(uint8_t cnt, int8_t dir) { void calcAvgSpeed(void) { // Calculate measured average speed. The minus sign (-) is because motors spin in opposite directions #if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) - speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2; + speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot); #elif !defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) - speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot) / 2; + speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot); #elif defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) - speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot) / 2; + speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot); #elif defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) - speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot) / 2; + speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot); + #endif + + speedAvg = 0; + #if defined(MOTOR_LEFT_ENA) + #if defined(INVERT_L_DIRECTION) + speedAvg -= rtY_Left.n_mot; + #else + speedAvg += rtY_Left.n_mot; + #endif + #endif + #if defined(MOTOR_RIGHT_ENA) + #if defined(INVERT_R_DIRECTION) + speedAvg += rtY_Right.n_mot; + #else + speedAvg -= rtY_Right.n_mot; + #endif + + // Average only if both motors are enabled + #if defined(MOTOR_LEFT_ENA) + speedAvg /= 2; + #endif #endif // Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1)