feedc0de/bobbycar-foc-model
1
0
Fork 0
forked from EFeru/hoverboard-firmware-hack-FOC
Code Pull Requests Activity

Fix average speed

Browse Source
This commit is contained in:
Candas1
2022-04-25 21:11:55 +02:00
parent bb119d7a38
commit 5612af0a90
3 changed files with 29 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Inc/config.h
View File

@ -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

6
Src/comms.c
View File

@ -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;

29
Src/util.c
View File

@ -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)