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:
- 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
#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
// Control selections
#define CTRL_TYP_SEL 2 // [-] Control type selection: 0 = Commutation , 1 = Sinusoidal, 2 = FOC Field Oriented Control (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_TYP_SEL FOC_CTRL // [-] Control type selection: COM_CTRL, SIN_CTRL, FOC_CTRL (default)
#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)
// 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 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.
// 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 ########################
@ -353,7 +368,7 @@
// ############################ VARIANT_HOVERCAR SETTINGS ############################
#ifdef VARIANT_HOVERCAR
#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 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
@ -369,6 +384,12 @@
#define SIDEBOARD_SERIAL_USART3
#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!
// 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
// Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses)

View File

@ -70,6 +70,8 @@ void adcCalibLim(void);
void updateCurSpdLim(void);
void saveConfig(void);
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
void poweroff(void);

View File

@ -46,9 +46,23 @@ In this firmware 3 control types are available:
- Commutation
- SIN (Sinusoidal)
- FOC (Field Oriented Control) with the following 3 control modes:
- **VOLTAGE MODE**: in this mode the controller applies a constant Voltage to the motors
- **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
- **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`.
- **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. 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.
#### 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)

View File

@ -146,7 +146,7 @@ static int16_t speed; // local variable for speed. -1000 to 10
#endif
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) {
@ -213,35 +213,33 @@ int main(void) {
}
// ####### VARIANT_HOVERCAR #######
#ifdef VARIANT_HOVERCAR
// 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]
#if defined(VARIANT_HOVERCAR) || defined(ELECTRIC_BRAKE_ENABLE)
uint16_t speedBlend; // Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15)
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
if (speedAvgAbs < 60) {
multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBreak); // Break pedal in this case is "cmd1" variable
#ifdef VARIANT_HOVERCAR
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(), &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 > 20) {
if (cmd1 > 30) { // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving
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)
if (speedAvg > 0) {
#ifdef ELECTRIC_BRAKE_ENABLE
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);
} else {
cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15);
}
#endif
// ####### GENERAL TIMEOUT #######
if (timeoutCnt > TIMEOUT) { // Bring the system to a Safe State
cmd1 = 0;
cmd2 = 0;
}
// ####### LOW-PASS FILTER #######
rateLimiter16(cmd1, RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt);
@ -250,12 +248,16 @@ int main(void) {
steer = (int16_t)(steerFixdt >> 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 #######
#ifdef VARIANT_HOVERCAR
if (!MultipleTapBreak.b_multipleTap) { // Check driving direction
speed = steer + speed; // Forward driving
if (!MultipleTapBrake.b_multipleTap) { // Check driving direction
speed = steer + speed; // Forward driving: in this case steer = Brake, speed = Throttle
} else {
speed = steer - speed; // Reverse driving
speed = steer - speed; // Reverse driving: in this case steer = Brake, speed = Throttle
}
#endif
@ -470,7 +472,7 @@ int main(void) {
} 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) || MultipleTapBreak.b_multipleTap)) { // backward beep
} else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // backward beep
buzzerFreq = 5;
buzzerPattern = 1;
backwardDrive = 1;

View File

@ -452,7 +452,7 @@ void adcCalibLim(void) {
adc_cal_valid = 1;
// 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_rx2, FILTER, &adc2_fixdt);
ADC1_MID_temp = (uint16_t)CLAMP(adc1_fixdt >> 16, 0, 4095); // convert fixed-point to integer
@ -461,7 +461,6 @@ void adcCalibLim(void) {
ADC1_MAX_temp = MAX(ADC1_MAX_temp, ADC1_MID_temp);
ADC2_MIN_temp = MIN(ADC2_MIN_temp, ADC2_MID_temp);
ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp);
adc_cal_timeout++;
HAL_Delay(5);
}
@ -515,10 +514,9 @@ void updateCurSpdLim(void) {
uint16_t spd_factor; // fixdt(0,16,16)
// 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_rx2, FILTER, &adc2_fixdt);
cur_spd_timeout++;
HAL_Delay(5);
}
@ -587,6 +585,65 @@ int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max) {
#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 =========================== */
@ -732,14 +789,6 @@ void readCommand(void) {
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
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
@ -764,14 +813,6 @@ void readCommand(void) {
}
#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)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
@ -812,6 +853,14 @@ void readCommand(void) {
#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; }
switch (sensor1_index) {
case 0: // FOC VOLTAGE
rtP_Left.z_ctrlTypSel = 2;
rtP_Right.z_ctrlTypSel = 2;
ctrlModReqRaw = 1;
rtP_Left.z_ctrlTypSel = FOC_CTRL;
rtP_Right.z_ctrlTypSel = FOC_CTRL;
ctrlModReqRaw = VLT_MODE;
break;
case 1: // FOC SPEED
ctrlModReqRaw = 2;
ctrlModReqRaw = SPD_MODE;
break;
case 2: // FOC TORQUE
ctrlModReqRaw = 3;
ctrlModReqRaw = TRQ_MODE;
break;
case 3: // SINUSOIDAL
rtP_Left.z_ctrlTypSel = 1;
rtP_Right.z_ctrlTypSel = 1;
rtP_Left.z_ctrlTypSel = SIN_CTRL;
rtP_Right.z_ctrlTypSel = SIN_CTRL;
break;
case 4: // COMMUTATION
rtP_Left.z_ctrlTypSel = 0;
rtP_Right.z_ctrlTypSel = 0;
rtP_Left.z_ctrlTypSel = COM_CTRL;
rtP_Right.z_ctrlTypSel = COM_CTRL;
break;
}
shortBeepMany(sensor1_index + 1);