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

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
This commit is contained in:
EmanuelFeru 2020-11-28 10:38:17 +01:00
parent cc1626e4fd
commit 098ca9f97c
5 changed files with 115 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
Inc/util.h
View File

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

24
README.md
View File

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

18
Src/bldc.c
View File

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

56
Src/main.c
View File

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

91
Src/util.c
View File

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