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

manually apply some improvements from current emanuelferu repo

This commit is contained in:
interfisch 2020-02-02 14:27:36 +01:00
parent 5f063a0bb6
commit 7bc84cf3e0
1 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
Src/main.c
View File

@ -118,7 +118,9 @@ static int16_t speedRightFixdt; // local fixed-point variable for speedRi
static int16_t speedLeftRateFixdt; // local fixed-point variable for steering 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 int16_t speedRightRateFixdt; // local fixed-point variable for speed rate limiter
static int16_t speed; // local variable for speed. -1000 to 1000. only used for security checks. will be calculated by speedL and speedR //static int16_t cmdspeedAvg; // local variable for commanded speed. -1000 to 1000. only used for security checks. will be calculated by speedL and speedR
static int16_t speedAvg; // average measured speed
static int16_t speedAvgAbs; // average measured speed in absolute
extern volatile int pwml; // global variable for pwm left. -1000 to 1000 extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000 extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
@ -135,6 +137,7 @@ extern int16_t curL_DC; // global variable for left motor curren
extern int16_t curR_DC; // global variable for right motor current extern int16_t curR_DC; // global variable for right motor current
static uint32_t inactivity_timeout_counter; static uint32_t inactivity_timeout_counter;
static uint32_t main_loop_counter;
extern uint8_t nunchuck_data[6]; extern uint8_t nunchuck_data[6];
#ifdef CONTROL_PPM #ifdef CONTROL_PPM
@ -372,7 +375,7 @@ int main(void) {
} }
// Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync. // Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync.
// Try to re-sync by reseting the DMA // Try to re-sync by reseting the DMA
if (command.start != START_FRAME && command.start != 0xFFFF) { if (main_loop_counter % 25 == 0 && command.start != START_FRAME && command.start != 0xFFFF) {
HAL_UART_DMAStop(&huart); HAL_UART_DMAStop(&huart);
HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command));
} }
@ -390,6 +393,24 @@ int main(void) {
#endif #endif
// Calculate measured average speed. The minus sign (-) is beacause motors spin in opposite directions
#if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION)
speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2;
#elif !defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION)
speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot) / 2;
#elif defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION)
speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot) / 2;
#elif defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION)
speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot) / 2;
#endif
// Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1)
if ((SPEED_COEFFICIENT & (1 << 16)) >> 16) {
speedAvg = -speedAvg;
}
speedAvgAbs = abs(speedAvg);
// ####### LOW-PASS FILTER ####### // ####### LOW-PASS FILTER #######
@ -408,7 +429,7 @@ int main(void) {
speedL = speedLeftFixdt >> 4; // convert fixed-point to integer speedL = speedLeftFixdt >> 4; // convert fixed-point to integer
speedR = speedRightFixdt >> 4; // convert fixed-point to integer speedR = speedRightFixdt >> 4; // convert fixed-point to integer
speed = (abs(speedL)+abs(speedR))/2; //cmdspeedAvg = (abs(speedL)+abs(speedR))/2;
// ####### MIXER ####### // ####### MIXER #######
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000); // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
@ -495,7 +516,7 @@ int main(void) {
// ####### BEEP AND EMERGENCY POWEROFF ####### // ####### BEEP AND EMERGENCY POWEROFF #######
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3 if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_LOW_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3
poweroff(); poweroff();
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot
buzzerFreq = 4; buzzerFreq = 4;
@ -509,7 +530,7 @@ int main(void) {
} else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep } else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep
buzzerFreq = 12; buzzerFreq = 12;
buzzerPattern = 1; buzzerPattern = 1;
} else if (BEEPS_BACKWARD && speed < -50) { // backward beep } else if (BEEPS_BACKWARD && speedAvg < -50) { // backward beep
buzzerFreq = 5; buzzerFreq = 5;
buzzerPattern = 1; buzzerPattern = 1;
} else { // do not beep } else { // do not beep
@ -527,6 +548,8 @@ int main(void) {
if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms
poweroff(); poweroff();
} }
main_loop_counter++;
} }
} }