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

4
Src/comms.c
View File

@@ -487,8 +487,7 @@ 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
@@ -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)