From 7bc84cf3e0e73a2dfc1cc16fd41a90664728ce8d Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 2 Feb 2020 14:27:36 +0100 Subject: [PATCH] manually apply some improvements from current emanuelferu repo --- Src/main.c | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/Src/main.c b/Src/main.c index 96150d2..2525201 100644 --- a/Src/main.c +++ b/Src/main.c @@ -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 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 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 static uint32_t inactivity_timeout_counter; +static uint32_t main_loop_counter; extern uint8_t nunchuck_data[6]; #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. // 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_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); } @@ -390,6 +393,24 @@ int main(void) { #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 ####### @@ -408,7 +429,7 @@ int main(void) { speedL = speedLeftFixdt >> 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 ####### // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000); @@ -495,7 +516,7 @@ int main(void) { // ####### 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(); } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot 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 buzzerFreq = 12; buzzerPattern = 1; - } else if (BEEPS_BACKWARD && speed < -50) { // backward beep + } else if (BEEPS_BACKWARD && speedAvg < -50) { // backward beep buzzerFreq = 5; buzzerPattern = 1; } 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 poweroff(); } + + main_loop_counter++; } }