Added functionality: Electric Brake, Standstill hold

- For TORQUE mode, by enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.
- For VOLTAGE and TORQUE mode, the standstill hold functionality can be forced by enabling `STANDSTILL_HOLD_ENABLE` in `config.h`.

Known (minor) issue: There is a small "tick" noise when Stanstill is engaged/disengaged, due to the switching to SPEED mode. To be solved by an improved mode switching strategy in the future.
This commit is contained in:
EmanuelFeru 2020-07-19 11:24:37 +02:00
parent 22984a7fd6
commit f2d86f3b30
5 changed files with 150 additions and 62 deletions

View File

@ -124,13 +124,22 @@
Outputs: Outputs:
- speedR and speedL: normal driving INPUT_MIN to INPUT_MAX - speedR and speedL: normal driving INPUT_MIN to INPUT_MAX
*/ */
#define COM_CTRL 0 // [-] Commutation Control Type
#define SIN_CTRL 1 // [-] Sinusoidal Control Type
#define FOC_CTRL 2 // [-] Field Oriented Control (FOC) Type
#define OPEN_MODE 0 // [-] OPEN mode
#define VLT_MODE 1 // [-] VOLTAGE mode
#define SPD_MODE 2 // [-] SPEED mode
#define TRQ_MODE 3 // [-] TORQUE mode
// Enable/Disable Motor // Enable/Disable Motor
#define MOTOR_LEFT_ENA // [-] Enable LEFT motor. Comment-out if this motor is not needed to be operational #define MOTOR_LEFT_ENA // [-] Enable LEFT motor. Comment-out if this motor is not needed to be operational
#define MOTOR_RIGHT_ENA // [-] Enable RIGHT motor. Comment-out if this motor is not needed to be operational #define MOTOR_RIGHT_ENA // [-] Enable RIGHT motor. Comment-out if this motor is not needed to be operational
// Control selections // Control selections
#define CTRL_TYP_SEL 2 // [-] Control type selection: 0 = Commutation , 1 = Sinusoidal, 2 = FOC Field Oriented Control (default) #define CTRL_TYP_SEL FOC_CTRL // [-] Control type selection: COM_CTRL, SIN_CTRL, FOC_CTRL (default)
#define CTRL_MOD_REQ 1 // [-] Control mode request: 0 = Open mode, 1 = VOLTAGE mode (default), 2 = SPEED mode, 3 = TORQUE mode. Note: SPEED and TORQUE modes are only available for FOC! #define CTRL_MOD_REQ VLT_MODE // [-] Control mode request: OPEN_MODE, VLT_MODE (default), SPD_MODE, TRQ_MODE. Note: SPD_MODE and TRQ_MODE are only available for CTRL_FOC!
#define DIAG_ENA 1 // [-] Motor Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default) #define DIAG_ENA 1 // [-] Motor Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default)
// Limitation settings // Limitation settings
@ -144,6 +153,12 @@
#define PHASE_ADV_MAX 25 // [deg] Maximum Phase Advance angle (only for SIN). Higher angle results in higher maximum speed. #define PHASE_ADV_MAX 25 // [deg] Maximum Phase Advance angle (only for SIN). Higher angle results in higher maximum speed.
#define FIELD_WEAK_HI 1500 // [-] Input target High threshold for reaching maximum Field Weakening / Phase Advance. Do NOT set this higher than 1500. #define FIELD_WEAK_HI 1500 // [-] Input target High threshold for reaching maximum Field Weakening / Phase Advance. Do NOT set this higher than 1500.
#define FIELD_WEAK_LO 1000 // [-] Input target Low threshold for starting Field Weakening / Phase Advance. Do NOT set this higher than 1000. #define FIELD_WEAK_LO 1000 // [-] Input target Low threshold for starting Field Weakening / Phase Advance. Do NOT set this higher than 1000.
// Extra functionality
// #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode.
// #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode.
// #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released).
// #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging.
// ########################### END OF MOTOR CONTROL ######################## // ########################### END OF MOTOR CONTROL ########################
@ -353,7 +368,7 @@
// ############################ VARIANT_HOVERCAR SETTINGS ############################ // ############################ VARIANT_HOVERCAR SETTINGS ############################
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
#undef CTRL_MOD_REQ #undef CTRL_MOD_REQ
#define CTRL_MOD_REQ 3 // HOVERCAR works best in TORQUE Mode #define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define ADC_PROTECT_ENA // ADC Protection Enable flag. Use this flag to make sure the ADC is protected when GND or Vcc wire is disconnected #define ADC_PROTECT_ENA // ADC Protection Enable flag. Use this flag to make sure the ADC is protected when GND or Vcc wire is disconnected
#define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken
@ -369,6 +384,12 @@
#define SIDEBOARD_SERIAL_USART3 #define SIDEBOARD_SERIAL_USART3
#define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
// #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
// Extra functionality
// #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode.
// #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode.
// #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released).
// #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging.
#endif #endif
// Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses) // Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses)

View File

@ -70,6 +70,8 @@ void adcCalibLim(void);
void updateCurSpdLim(void); void updateCurSpdLim(void);
void saveConfig(void); void saveConfig(void);
int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max); int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max);
void standstillHold(int16_t *speedCmd);
void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
// Poweroff Functions // Poweroff Functions
void poweroff(void); void poweroff(void);

View File

@ -46,10 +46,24 @@ In this firmware 3 control types are available:
- Commutation - Commutation
- SIN (Sinusoidal) - SIN (Sinusoidal)
- FOC (Field Oriented Control) with the following 3 control modes: - FOC (Field Oriented Control) with the following 3 control modes:
- **VOLTAGE MODE**: in this mode the controller applies a constant Voltage to the motors - **VOLTAGE MODE**: in this mode the controller applies a constant Voltage to the motors. Recommended for robotics applications or applications where a fast motor response is required.
- **SPEED MODE**: in this mode a closed-loop controller realizes the input speed target by rejecting any of the disturbance (resistive load) applied to the motor - **SPEED MODE**: in this mode a closed-loop controller realizes the input speed target by rejecting any of the disturbance (resistive load) applied to the motor. Recommended for robotics applications or constant speed applications.
- **TORQUE MODE**: in this mode the input torque target is realized. This mode enables motor "freewheeling" when the torque target is `0`. Recommended for most applications with a sitting human driver. If motor braking is desired instead of "freewheel" when torque target is `0`, then a torque target below `0` should be set when `speedAvgAbs > 0`. - **TORQUE MODE**: in this mode the input torque target is realized. This mode enables motor "freewheeling" when the torque target is `0`. Recommended for most applications with a sitting human driver.
#### Comparison between different control methods
|Control method| Complexity | Efficiency | Smoothness | Field Weakening | Freewheeling | Standstill hold |
|--|--|--|--|--|--|--|
|Commutation| - | - | ++ | n.a. | n.a. | + |
|Sinusoidal| + | ++ | ++ | +++ | n.a. | + |
|FOC VOLTAGE| ++ | +++ | ++ | ++ | n.a. | +<sup>(2)</sup> |
|FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ |
|FOC TORQUE| +++ | +++ | +++ | ++ | +++<sup>(1)</sup> | n.a<sup>(2)</sup> |
<sup>(1)</sup> By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.
<sup>(2)</sup> The standstill hold functionality can be forced by enabling `STANDSTILL_HOLD_ENABLE` in `config.h`.
![Schematic representation of the available control methods](/01_Matlab/02_Figures/control_methods.png) ![Schematic representation of the available control methods](/01_Matlab/02_Figures/control_methods.png)
In all FOC control modes, the controller features maximum motor speed and maximum motor current protection. This brings great advantages to fulfil the needs of many robotic applications while maintaining safe operation. In all FOC control modes, the controller features maximum motor speed and maximum motor current protection. This brings great advantages to fulfil the needs of many robotic applications while maintaining safe operation.

View File

@ -146,7 +146,7 @@ static int16_t speed; // local variable for speed. -1000 to 10
#endif #endif
static uint32_t inactivity_timeout_counter; static uint32_t inactivity_timeout_counter;
static MultipleTap MultipleTapBreak; // define multiple tap functionality for the Break pedal static MultipleTap MultipleTapBrake; // define multiple tap functionality for the Brake pedal
int main(void) { int main(void) {
@ -213,49 +213,51 @@ int main(void) {
} }
// ####### VARIANT_HOVERCAR ####### // ####### VARIANT_HOVERCAR #######
#ifdef VARIANT_HOVERCAR #if defined(VARIANT_HOVERCAR) || defined(ELECTRIC_BRAKE_ENABLE)
// Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15) uint16_t speedBlend; // Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15)
uint16_t speedBlend; speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // speedBlend [0,1] is within [10 rpm, 60rpm]
speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // speedBlend [0,1] is within [10 rpm, 60rpm] #endif
// Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality #ifdef VARIANT_HOVERCAR
if (speedAvgAbs < 60) { if (speedAvgAbs < 60) { // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality
multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBreak); // Break pedal in this case is "cmd1" variable multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "cmd1" variable
} }
// If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving if (cmd1 > 30) { // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving
if (cmd1 > 20) {
cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); cmd2 = (int16_t)((cmd2 * speedBlend) >> 15);
} }
#endif
// Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal) #ifdef ELECTRIC_BRAKE_ENABLE
if (speedAvg > 0) { electricBrake(speedBlend, MultipleTapBrake.b_multipleTap); // Apply Electric Brake. Only available and makes sense for TORQUE Mode
#endif
#ifdef VARIANT_HOVERCAR
if (speedAvg > 0) { // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal)
cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15); cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15);
} else { } else {
cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15); cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15);
} }
#endif #endif
// ####### GENERAL TIMEOUT #######
if (timeoutCnt > TIMEOUT) { // Bring the system to a Safe State
cmd1 = 0;
cmd2 = 0;
}
// ####### LOW-PASS FILTER ####### // ####### LOW-PASS FILTER #######
rateLimiter16(cmd1, RATE, &steerRateFixdt); rateLimiter16(cmd1, RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt); rateLimiter16(cmd2, RATE, &speedRateFixdt);
filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer
speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer
#ifdef STANDSTILL_HOLD_ENABLE
standstillHold(&speed); // Apply Standstill Hold functionality. Only available and makes sense for VOLTAGE or TORQUE Mode
#endif
// ####### VARIANT_HOVERCAR ####### // ####### VARIANT_HOVERCAR #######
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
if (!MultipleTapBreak.b_multipleTap) { // Check driving direction if (!MultipleTapBrake.b_multipleTap) { // Check driving direction
speed = steer + speed; // Forward driving speed = steer + speed; // Forward driving: in this case steer = Brake, speed = Throttle
} else { } else {
speed = steer - speed; // Reverse driving speed = steer - speed; // Reverse driving: in this case steer = Brake, speed = Throttle
} }
#endif #endif
@ -470,7 +472,7 @@ int main(void) {
} else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // low bat 2: slow beep } else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // low bat 2: slow beep
buzzerFreq = 5; buzzerFreq = 5;
buzzerPattern = 42; buzzerPattern = 42;
} else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBreak.b_multipleTap)) { // backward beep } else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // backward beep
buzzerFreq = 5; buzzerFreq = 5;
buzzerPattern = 1; buzzerPattern = 1;
backwardDrive = 1; backwardDrive = 1;

View File

@ -452,7 +452,7 @@ void adcCalibLim(void) {
adc_cal_valid = 1; adc_cal_valid = 1;
// Extract MIN, MAX and MID from ADC while the power button is not pressed // Extract MIN, MAX and MID from ADC while the power button is not pressed
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && adc_cal_timeout < 4000) { // 20 sec timeout while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && adc_cal_timeout++ < 4000) { // 20 sec timeout
filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt);
filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt);
ADC1_MID_temp = (uint16_t)CLAMP(adc1_fixdt >> 16, 0, 4095); // convert fixed-point to integer ADC1_MID_temp = (uint16_t)CLAMP(adc1_fixdt >> 16, 0, 4095); // convert fixed-point to integer
@ -460,8 +460,7 @@ void adcCalibLim(void) {
ADC1_MIN_temp = MIN(ADC1_MIN_temp, ADC1_MID_temp); ADC1_MIN_temp = MIN(ADC1_MIN_temp, ADC1_MID_temp);
ADC1_MAX_temp = MAX(ADC1_MAX_temp, ADC1_MID_temp); ADC1_MAX_temp = MAX(ADC1_MAX_temp, ADC1_MID_temp);
ADC2_MIN_temp = MIN(ADC2_MIN_temp, ADC2_MID_temp); ADC2_MIN_temp = MIN(ADC2_MIN_temp, ADC2_MID_temp);
ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp); ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp);
adc_cal_timeout++;
HAL_Delay(5); HAL_Delay(5);
} }
@ -515,10 +514,9 @@ void updateCurSpdLim(void) {
uint16_t spd_factor; // fixdt(0,16,16) uint16_t spd_factor; // fixdt(0,16,16)
// Wait for the power button press // Wait for the power button press
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout < 2000) { // 10 sec timeout while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt);
filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt);
cur_spd_timeout++;
HAL_Delay(5); HAL_Delay(5);
} }
@ -587,6 +585,65 @@ int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max) {
#endif #endif
} }
/*
* Standstill Hold Function
* This function will switch to SPEED mode at standstill to provide an anti-roll functionality.
* Only available and makes sense for VOLTAGE or TORQUE mode.
*
* Input: pointer *speedCmd
* Output: modified Control Mode Request
*/
void standstillHold(int16_t *speedCmd) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE)
if (*speedCmd > -20 && *speedCmd < 20) { // If speedCmd (Throttle) is small
if (ctrlModReqRaw != SPD_MODE && speedAvgAbs < 3) { // and If measured speed is small (meaning we are at standstill)
ctrlModReqRaw = SPD_MODE; // Switch to Speed mode
}
if (ctrlModReqRaw == SPD_MODE) { // If we are in Speed mode
*speedCmd = 0; // Request standstill (0 rpm)
}
} else if (ctrlModReqRaw != CTRL_MOD_REQ && (*speedCmd < -50 || *speedCmd > 50)) { // Else if speedCmd (Throttle) becomes significant
ctrlModReqRaw = CTRL_MOD_REQ; // Follow the Mode request
}
#endif
}
/*
* Electric Brake Function
* In case of TORQUE mode, this function replaces the motor "freewheel" with a constant braking when the input torque request is 0.
* This is useful when a small amount of motor braking is desired instead of "freewheel".
*
* Input: speedBlend = fixdt(0,16,15), reverseDir = {0, 1}
* Output: cmd2 (Throtle) with brake component included
*/
void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
#if defined(ELECTRIC_BRAKE_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ == TRQ_MODE)
int16_t brakeVal;
// Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving)
if (speedAvg > 0) {
brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
} else {
brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
}
// Check if direction is reversed
if (reverseDir) {
brakeVal = -brakeVal;
}
// Calculate the new cmd2 with brake component included
if (cmd2 >= 0 && cmd2 < ELECTRIC_BRAKE_THRES) {
cmd2 = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - cmd2) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (cmd2 >= -ELECTRIC_BRAKE_THRES && cmd2 < 0) {
cmd2 = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + cmd2) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (cmd2 >= ELECTRIC_BRAKE_THRES) {
cmd2 = MAX(brakeVal, ((cmd2 - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES));
} else { // when (cmd2 < -ELECTRIC_BRAKE_THRES)
cmd2 = MIN(brakeVal, ((cmd2 + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES));
}
#endif
}
/* =========================== Poweroff Functions =========================== */ /* =========================== Poweroff Functions =========================== */
@ -731,15 +788,7 @@ void readCommand(void) {
timeoutFlagADC = 1; // Timeout detected timeoutFlagADC = 1; // Timeout detected
timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value
} }
} }
if (timeoutFlagADC) { // In case of timeout bring the system to a Safe State
ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way
cmd1 = 0;
cmd2 = 0;
} else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
}
#endif #endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) #if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
@ -764,14 +813,6 @@ void readCommand(void) {
} }
#endif #endif
if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State
ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way
cmd1 = 0;
cmd2 = 0;
} else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
}
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) #if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN); button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN); button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
@ -812,6 +853,14 @@ void readCommand(void) {
#endif #endif
#endif #endif
if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State
ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way
cmd1 = 0;
cmd2 = 0;
} else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
}
} }
@ -1142,23 +1191,23 @@ void sideboardSensors(uint8_t sensors) {
if (sensor1_index > 4) { sensor1_index = 0; } if (sensor1_index > 4) { sensor1_index = 0; }
switch (sensor1_index) { switch (sensor1_index) {
case 0: // FOC VOLTAGE case 0: // FOC VOLTAGE
rtP_Left.z_ctrlTypSel = 2; rtP_Left.z_ctrlTypSel = FOC_CTRL;
rtP_Right.z_ctrlTypSel = 2; rtP_Right.z_ctrlTypSel = FOC_CTRL;
ctrlModReqRaw = 1; ctrlModReqRaw = VLT_MODE;
break; break;
case 1: // FOC SPEED case 1: // FOC SPEED
ctrlModReqRaw = 2; ctrlModReqRaw = SPD_MODE;
break; break;
case 2: // FOC TORQUE case 2: // FOC TORQUE
ctrlModReqRaw = 3; ctrlModReqRaw = TRQ_MODE;
break; break;
case 3: // SINUSOIDAL case 3: // SINUSOIDAL
rtP_Left.z_ctrlTypSel = 1; rtP_Left.z_ctrlTypSel = SIN_CTRL;
rtP_Right.z_ctrlTypSel = 1; rtP_Right.z_ctrlTypSel = SIN_CTRL;
break; break;
case 4: // COMMUTATION case 4: // COMMUTATION
rtP_Left.z_ctrlTypSel = 0; rtP_Left.z_ctrlTypSel = COM_CTRL;
rtP_Right.z_ctrlTypSel = 0; rtP_Right.z_ctrlTypSel = COM_CTRL;
break; break;
} }
shortBeepMany(sensor1_index + 1); shortBeepMany(sensor1_index + 1);