This commit is contained in:
Candas1 2020-11-15 20:04:47 +01:00
commit 75082c8333
3 changed files with 35 additions and 22 deletions

View File

@ -71,7 +71,7 @@ int checkInputType(int16_t min, int16_t mid, int16_t max);
void updateCurSpdLim(void);
void saveConfig(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);
void standstillHold(int16_t *speedCmd);
void standstillHold(void);
void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
void cruiseControl(uint8_t button);

View File

@ -220,6 +220,10 @@ int main(void) {
speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // speedBlend [0,1] is within [10 rpm, 60rpm]
#endif
#ifdef STANDSTILL_HOLD_ENABLE
standstillHold(); // Apply Standstill Hold functionality. Only available and makes sense for VOLTAGE or TORQUE Mode
#endif
#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
@ -261,10 +265,6 @@ 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 (!MultipleTapBrake.b_multipleTap) { // Check driving direction

View File

@ -192,6 +192,10 @@ static uint8_t button2; // Green
static uint8_t brakePressed;
#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 =========================== */
@ -657,23 +661,30 @@ int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16
/*
* 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.
* This function uses Cruise Control to provide an anti-roll functionality at standstill.
* Only available and makes sense for FOC VOLTAGE or FOC TORQUE mode.
*
* Input: pointer *speedCmd
* Output: modified Control Mode Request
* Input: none
* 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 (*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 (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation
if (((cmd1 > 50 || cmd2 < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (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;
}
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
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;
}
}
#endif
}
@ -720,7 +731,7 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
* This function activates/deactivates cruise control.
*
* Input: button (as a pulse)
* Output: none
* Output: cruiseCtrlAcv
*/
void cruiseControl(uint8_t button) {
#ifdef CRUISE_CONTROL_SUPPORT
@ -729,10 +740,12 @@ void cruiseControl(uint8_t button) {
rtP_Right.n_cruiseMotTgt = rtY_Right.n_mot;
rtP_Left.b_cruiseCtrlEna = 1;
rtP_Right.b_cruiseCtrlEna = 1;
cruiseCtrlAcv = 1;
shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also.
} else if (button && rtP_Left.b_cruiseCtrlEna) { // Cruise control deactivated
} else if (button && rtP_Left.b_cruiseCtrlEna && !standstillAcv) { // Cruise control deactivated if no Standstill Hold is active
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
cruiseCtrlAcv = 0;
shortBeepMany(2, -1);
}
#endif