From 098ca9f97cb0f7ec9a4e6f5b9583ed341a47caea Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Sat, 28 Nov 2020 10:38:17 +0100 Subject: [PATCH] Diag: beep numbers for errors - implemented beep numbers in case of error for easier debugging - fixed bug: power button needs to be pressed multiple times to turn on --- Inc/util.h | 23 +++++++------- README.md | 24 ++++++++------ Src/bldc.c | 18 ++++++++--- Src/main.c | 56 +++++++++++++++------------------ Src/util.c | 91 +++++++++++++++++++++++++++++------------------------- 5 files changed, 115 insertions(+), 97 deletions(-) diff --git a/Inc/util.h b/Inc/util.h index 54e0fe6..0037f9c 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -26,20 +26,20 @@ // Rx Structures USART #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) - #ifdef CONTROL_IBUS + #ifdef CONTROL_IBUS typedef struct{ uint8_t start; uint8_t type; uint8_t channels[IBUS_NUM_CHANNELS*2]; uint8_t checksuml; - uint8_t checksumh; + uint8_t checksumh; } SerialCommand; #else typedef struct{ - uint16_t start; + uint16_t start; int16_t steer; int16_t speed; - uint16_t checksum; + uint16_t checksum; } SerialCommand; #endif #endif @@ -62,9 +62,10 @@ void UART_DisableRxErrors(UART_HandleTypeDef *huart); // General Functions void poweronMelody(void); -void shortBeep(uint8_t freq); -void shortBeepMany(uint8_t cnt, int8_t dir); -void longBeep(uint8_t freq); +void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern); +void beepLong(uint8_t freq); +void beepShort(uint8_t freq); +void beepShortMany(uint8_t cnt, int8_t dir); void calcAvgSpeed(void); int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max); int checkInputType(int16_t min, int16_t mid, int16_t max); @@ -105,10 +106,10 @@ void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t // Multiple Tap Function typedef struct { - uint32_t t_timePrev; - uint8_t z_pulseCntPrev; - uint8_t b_hysteresis; - uint8_t b_multipleTap; + uint32_t t_timePrev; + uint8_t z_pulseCntPrev; + uint8_t b_hysteresis; + uint8_t b_multipleTap; } MultipleTap; void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x); diff --git a/README.md b/README.md index ec5398e..8f5af8a 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,7 @@ Table of Contents * [FOC Firmware](#foc-firmware) * [Example Variants ](#example-variants) * [Flashing](#flashing) +* [Diagnostics](#diagnostics) * [Troubleshooting](#troubleshooting) * [Projects and Links](#projects-and-links) * [Contributions](#contributions) @@ -100,15 +101,6 @@ In all FOC control modes, the controller features maximum motor speed and maximu - The controller parameters are given in [this table](https://github.com/EmanuelFeru/bldc-motor-control-FOC/blob/master/02_Figures/paramTable.png) -### Diagnostics -Each motor is constantly monitored for errors. These errors are: -- **Error 001**: Hall sensor not connected -- **Error 002**: Hall sensor short circuit -- **Error 004**: Motor NOT able to spin (Possible causes: motor phase disconnected, MOSFET defective, operational Amplifier defective, motor blocked) - -The error codes above are reported for each motor in the variables **rtY_Left.z_errCode** and **rtY_Right.z_errCode** for Left motor (long wired motor) and Right motor (short wired motor), respectively. In case of error, the motor power is reduced to 0, while an audible (fast beep) can be heard to notify the user. - - --- ## Example Variants @@ -208,6 +200,20 @@ platformio run –target upload -e VARIANT_#### ``` If you have set default_envs in [platformio.ini](/platformio.ini) you can ommit -e parameter + +--- +## Diagnostics +The errors reported by the board are in the form of audible beeps: +- **1 beep (low pitch)**: Motor error (see [possible causes](https://github.com/EmanuelFeru/bldc-motor-control-FOC#diagnostics)) +- **2 beeps (low pitch)**: ADC timeout +- **3 beeps (low pitch)**: Serial communication timeout +- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuck) +- **5 beeps (low pitch)**: Mainboard temperature warning +- **1 beep fast (medium pitch)**: Low battery voltage < 35V +- **1 beep slow (medium pitch)**: Low battery voltage < 36V +- **1 beep fast (high pitch)**: Backward spinning motors + + --- ## Troubleshooting First, check that power is connected and voltage is >36V while flashing. diff --git a/Src/bldc.c b/Src/bldc.c index 7de76b2..cb22ade 100644 --- a/Src/bldc.c +++ b/Src/bldc.c @@ -59,7 +59,10 @@ extern volatile adc_buf_t adc_buffer; uint8_t buzzerFreq = 0; uint8_t buzzerPattern = 0; +uint8_t buzzerCount = 0; static uint32_t buzzerTimer = 0; +static uint8_t buzzerPrev = 0; +static uint8_t buzzerIdx = 0; uint8_t enable = 0; // initially motors are disabled for SAFETY static uint8_t enableFin = 0; @@ -97,7 +100,7 @@ void DMA1_Channel1_IRQHandler(void) { return; } - if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point + if (buzzerTimer % 1000 == 0) { // Filter battery voltage at a slower sampling rate filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); batVoltage = (int16_t)(batVoltageFixdt >> 16); // convert fixed-point to integer } @@ -126,14 +129,21 @@ void DMA1_Channel1_IRQHandler(void) { RIGHT_TIM->BDTR |= TIM_BDTR_MOE; } - //create square wave for buzzer + // Create square wave for buzzer buzzerTimer++; if (buzzerFreq != 0 && (buzzerTimer / 5000) % (buzzerPattern + 1) == 0) { - if (buzzerTimer % buzzerFreq == 0) { + if (buzzerPrev == 0) { + buzzerPrev = 1; + if (++buzzerIdx > (buzzerCount + 2)) { // pause 2 periods + buzzerIdx = 1; + } + } + if (buzzerTimer % buzzerFreq == 0 && (buzzerIdx <= buzzerCount || buzzerCount == 0)) { HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN); } - } else { + } else if (buzzerPrev) { HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, GPIO_PIN_RESET); + buzzerPrev = 0; } // ############################### MOTOR CONTROL ############################### diff --git a/Src/main.c b/Src/main.c index 90afd7e..2b6956e 100644 --- a/Src/main.c +++ b/Src/main.c @@ -74,9 +74,6 @@ extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 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 uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7... -extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7... - extern uint8_t enable; // global variable for motor enable extern int16_t batVoltage; // global variable for battery voltage @@ -114,7 +111,7 @@ typedef struct{ int16_t speedL_meas; int16_t batVoltage; int16_t boardTemp; - uint16_t cmdLed; + uint16_t cmdLed; uint16_t checksum; } SerialFeedback; static SerialFeedback Feedback; @@ -180,13 +177,13 @@ int main(void) { MX_ADC1_Init(); MX_ADC2_Init(); BLDC_Init(); // BLDC Controller Init + + HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); // Activate Latch Input_Lim_Init(); // Input Limitations Init Input_Init(); // Input Init - HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); - HAL_ADC_Start(&hadc1); - HAL_ADC_Start(&hadc2); + HAL_ADC_Start(&hadc2); poweronMelody(); HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET); @@ -208,8 +205,8 @@ int main(void) { #ifndef VARIANT_TRANSPOTTER // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ - shortBeep(6); // make 2 beeps indicating the motor enable - shortBeep(4); HAL_Delay(100); + beepShort(6); // make 2 beeps indicating the motor enable + beepShort(4); HAL_Delay(100); steerFixdt = speedFixdt = 0; // reset filters enable = 1; // enable motors consoleLog("-- Motors enabled --\r\n"); @@ -348,7 +345,7 @@ int main(void) { if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away! enable = 0; - longBeep(5); + beepLong(5); #ifdef SUPPORT_LCD LCD_ClearDisplay(&lcd); HAL_Delay(5); @@ -460,29 +457,26 @@ int main(void) { // ####### BEEP AND EMERGENCY POWEROFF ####### if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3 poweroff(); - } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // disable motors and beep in case of Motor error - fast beep - enable = 0; - buzzerFreq = 8; - buzzerPattern = 1; - } else if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // beep in case of ADC timeout, Serial timeout or General timeout - fast beep - buzzerFreq = 24; - buzzerPattern = 1; - } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot - buzzerFreq = 4; - buzzerPattern = 1; - } else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) { // low bat 1: fast beep - buzzerFreq = 5; - buzzerPattern = 6; - } else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // low bat 2: slow beep - buzzerFreq = 5; - buzzerPattern = 42; - } else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // backward beep - buzzerFreq = 5; - buzzerPattern = 1; + } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors + enable = 0; + beepCount(1, 24, 1); + } else if (timeoutFlagADC) { // 2 beeps (low pitch): ADC timeout + beepCount(2, 24, 1); + } else if (timeoutFlagSerial) { // 3 beeps (low pitch): Serial timeout + beepCount(3, 24, 1); + } else if (timeoutCnt > TIMEOUT) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuck) + beepCount(4, 24, 1); + } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning + beepCount(5, 24, 1); + } else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) { // 1 beep fast (medium pitch): Low bat 1 + beepCount(0, 10, 6); + } else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2 + beepCount(0, 10, 30); + } else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1 beep fast (high pitch): Backward spinning motors + beepCount(0, 5, 1); backwardDrive = 1; } else { // do not beep - buzzerFreq = 0; - buzzerPattern = 0; + beepCount(0, 0, 0); backwardDrive = 0; } diff --git a/Src/util.c b/Src/util.c index 26d8e35..c9363c7 100644 --- a/Src/util.c +++ b/Src/util.c @@ -46,6 +46,7 @@ extern UART_HandleTypeDef huart3; extern int16_t batVoltage; extern uint8_t backwardDrive; +extern uint8_t buzzerCount; // global variable for the buzzer counts. can be 1, 2, 3, 4, 5, 6, 7... extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7... extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7... @@ -369,37 +370,43 @@ void UART_DisableRxErrors(UART_HandleTypeDef *huart) /* =========================== General Functions =========================== */ void poweronMelody(void) { - for (int i = 8; i >= 0; i--) { - buzzerFreq = (uint8_t)i; - HAL_Delay(100); - } - buzzerFreq = 0; + for (int i = 8; i >= 0; i--) { + buzzerFreq = (uint8_t)i; + HAL_Delay(100); + } + buzzerFreq = 0; } -void shortBeep(uint8_t freq) { +void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern) { + buzzerCount = cnt; + buzzerFreq = freq; + buzzerPattern = pattern; +} + +void beepLong(uint8_t freq) { + buzzerFreq = freq; + HAL_Delay(500); + buzzerFreq = 0; +} + +void beepShort(uint8_t freq) { buzzerFreq = freq; HAL_Delay(100); buzzerFreq = 0; } -void shortBeepMany(uint8_t cnt, int8_t dir) { +void beepShortMany(uint8_t cnt, int8_t dir) { if (dir >= 0) { // increasing tone for(uint8_t i = 2*cnt; i >= 2; i=i-2) { - shortBeep(i + 3); + beepShort(i + 3); } } else { // decreasing tone for(uint8_t i = 2; i <= 2*cnt; i=i+2) { - shortBeep(i + 3); + beepShort(i + 3); } } } -void longBeep(uint8_t freq) { - buzzerFreq = freq; - HAL_Delay(500); - buzzerFreq = 0; -} - 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) @@ -471,13 +478,13 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){ #ifdef CONTROL_ADC if ((min + INPUT_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - INPUT_MARGIN + ADC_PROTECT_THRESH) < 4095) { consoleLog(" and protected"); - longBeep(2); // Indicate protection by a beep + beepLong(2); // Indicate protection by a beep } #endif } HAL_Delay(10); - consoleLog("\n"); + consoleLog("\r\n"); HAL_Delay(10); return type; @@ -498,7 +505,7 @@ void adcCalibLim(void) { } #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - consoleLog("Input calibration started...\n"); + consoleLog("Input calibration started...\r\n"); readInput(); // Inititalization: MIN = a high value, MAX = a low value @@ -532,10 +539,10 @@ void adcCalibLim(void) { INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN; INPUT1_MID_CAL = INPUT1_MID_temp; INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN; - consoleLog("Input1 OK\n"); HAL_Delay(10); + consoleLog("Input1 OK\r\n"); HAL_Delay(10); } else { INPUT1_TYP_CAL = 0; // Disable input - consoleLog("Input1 Fail\n"); HAL_Delay(10); + consoleLog("Input1 Fail\r\n"); HAL_Delay(10); } INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp); @@ -543,10 +550,10 @@ void adcCalibLim(void) { INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN; INPUT2_MID_CAL = INPUT2_MID_temp; INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN; - consoleLog("Input2 OK\n"); HAL_Delay(10); + consoleLog("Input2 OK\r\n"); HAL_Delay(10); } else { INPUT2_TYP_CAL = 0; // Disable input - consoleLog("Input2 Fail\n"); HAL_Delay(10); + consoleLog("Input2 Fail\r\n"); HAL_Delay(10); } inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown @@ -575,7 +582,7 @@ void updateCurSpdLim(void) { } #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - consoleLog("Torque and Speed limits update started...\n"); + consoleLog("Torque and Speed limits update started...\r\n"); int32_t input1_fixdt = input1 << 16; int32_t input2_fixdt = input2 << 16; @@ -749,16 +756,16 @@ void cruiseControl(uint8_t button) { /* =========================== Poweroff Functions =========================== */ void poweroff(void) { - buzzerPattern = 0; - enable = 0; - consoleLog("-- Motors disabled --\r\n"); - for (int i = 0; i < 8; i++) { - buzzerFreq = (uint8_t)i; - HAL_Delay(100); - } + enable = 0; + consoleLog("-- Motors disabled --\r\n"); + buzzerPattern = 0; + for (int i = 0; i < 8; i++) { + buzzerFreq = (uint8_t)i; + HAL_Delay(100); + } saveConfig(); - HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); - while(1) {} + HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); + while(1) {} } @@ -769,19 +776,19 @@ void poweroffPressCheck(void) { uint16_t cnt_press = 0; while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); - if (cnt_press++ == 5 * 100) { shortBeep(5); } + if (cnt_press++ == 5 * 100) { beepShort(5); } } if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec HAL_Delay(1000); if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - longBeep(8); + beepLong(8); updateCurSpdLim(); - shortBeep(5); + beepShort(5); } else { // Long press: Calibrate ADC Limits - longBeep(16); + beepLong(16); adcCalibLim(); - shortBeep(5); + beepShort(5); } } else { // Short press: power off poweroff(); @@ -791,11 +798,11 @@ void poweroffPressCheck(void) { if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { enable = 0; while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - shortBeep(5); + beepShort(5); HAL_Delay(300); if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - longBeep(5); + beepLong(5); HAL_Delay(350); poweroff(); } else { @@ -803,7 +810,7 @@ void poweroffPressCheck(void) { if (setDistance > 2.6) { setDistance = 0.5; } - shortBeep(setDistance / 0.25); + beepShort(setDistance / 0.25); saveValue = setDistance * 1000; saveValue_valid = 1; } @@ -1307,7 +1314,7 @@ void sideboardSensors(uint8_t sensors) { rtP_Right.z_ctrlTypSel = COM_CTRL; break; } - shortBeepMany(sensor1_index + 1, 1); + beepShortMany(sensor1_index + 1, 1); } // Field Weakening: use Sensor2 as push button @@ -1332,7 +1339,7 @@ void sideboardSensors(uint8_t sensors) { Input_Lim_Init(); break; } - shortBeepMany(sensor2_index + 1, 1); + beepShortMany(sensor2_index + 1, 1); } #endif // CRUISE_CONTROL_SUPPORT #endif