Improved Standstill Hold functionality

- Standstill Hold should now engage sooner if Brake pedal is pressed to prevent continuous rolling in a Downhill situation.
This commit is contained in:
EmanuelFeru 2020-11-12 22:41:29 +01:00
parent 4fbf2549c9
commit c2647ed686
3 changed files with 35 additions and 22 deletions

View File

@ -70,7 +70,7 @@ 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 in_min, int16_t in_max, int16_t out_min, int16_t out_max); int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max);
void standstillHold(int16_t *speedCmd); void standstillHold(void);
void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
void cruiseControl(uint8_t button); void cruiseControl(uint8_t button);

View File

@ -218,6 +218,10 @@ int main(void) {
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 #endif
#ifdef STANDSTILL_HOLD_ENABLE
standstillHold(); // Apply Standstill Hold functionality. Only available and makes sense for VOLTAGE or TORQUE Mode
#endif
#ifdef VARIANT_HOVERCAR #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 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 multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "cmd1" variable
@ -259,10 +263,6 @@ int main(void) {
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 (!MultipleTapBrake.b_multipleTap) { // Check driving direction if (!MultipleTapBrake.b_multipleTap) { // Check driving direction

View File

@ -195,6 +195,10 @@ static uint8_t button2; // Green
static uint8_t brakePressed; static uint8_t brakePressed;
#endif #endif
#if defined(CRUISE_CONTROL_SUPPORT) || (defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE))
static uint8_t cruiseCtrlAcv = 0;
static uint8_t standstillAcv = 0;
#endif
/* =========================== Initialization Functions =========================== */ /* =========================== Initialization Functions =========================== */
@ -583,23 +587,30 @@ int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int
/* /*
* Standstill Hold Function * Standstill Hold Function
* This function will switch to SPEED mode at standstill to provide an anti-roll functionality. * This function uses Cruise Control to provide an anti-roll functionality at standstill.
* Only available and makes sense for VOLTAGE or TORQUE mode. * Only available and makes sense for FOC VOLTAGE or FOC TORQUE mode.
* *
* Input: pointer *speedCmd * Input: none
* Output: modified Control Mode Request * Output: standstillAcv
*/ */
void standstillHold(int16_t *speedCmd) { void standstillHold(void) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE) #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 (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation
if (ctrlModReqRaw != SPD_MODE && speedAvgAbs < 3) { // and If measured speed is small (meaning we are at standstill) if (((cmd1 > 50 || cmd2 < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
ctrlModReqRaw = SPD_MODE; // Switch to Speed mode || (cmd2 < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
rtP_Left.n_cruiseMotTgt = 0;
rtP_Right.n_cruiseMotTgt = 0;
rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1;
standstillAcv = 1;
}
}
else { // If Stanstill is Active -> try Deactivation
if (cmd1 < 20 && cmd2 > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
standstillAcv = 0;
} }
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 #endif
} }
@ -646,19 +657,21 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
* This function activates/deactivates cruise control. * This function activates/deactivates cruise control.
* *
* Input: button (as a pulse) * Input: button (as a pulse)
* Output: none * Output: cruiseCtrlAcv
*/ */
void cruiseControl(uint8_t button) { void cruiseControl(uint8_t button) {
#ifdef CRUISE_CONTROL_SUPPORT #ifdef CRUISE_CONTROL_SUPPORT
if (button && !rtP_Left.b_cruiseCtrlEna) { // Cruise control activated if (button && !rtP_Left.b_cruiseCtrlEna) { // Cruise control activated
rtP_Left.n_cruiseMotTgt = rtY_Left.n_mot; rtP_Left.n_cruiseMotTgt = rtY_Left.n_mot;
rtP_Right.n_cruiseMotTgt = rtY_Right.n_mot; rtP_Right.n_cruiseMotTgt = rtY_Right.n_mot;
rtP_Left.b_cruiseCtrlEna = 1; rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1; rtP_Right.b_cruiseCtrlEna = 1;
shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also. cruiseCtrlAcv = 1;
} else if (button && rtP_Left.b_cruiseCtrlEna) { // Cruise control deactivated shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also.
} else if (button && rtP_Left.b_cruiseCtrlEna && !standstillAcv) { // Cruise control deactivated if no Standstill Hold is active
rtP_Left.b_cruiseCtrlEna = 0; rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0; rtP_Right.b_cruiseCtrlEna = 0;
cruiseCtrlAcv = 0;
shortBeepMany(2, -1); shortBeepMany(2, -1);
} }
#endif #endif