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.

Inc/config.h

@@ -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_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)
 
 // 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)

Inc/util.h

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

README.md

@@ -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
+  - **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)
  

Src/main.c

@@ -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
+      #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);
         }
+      #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);
         } 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
+        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 {
-          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;

Src/util.c

@@ -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_Left.z_ctrlTypSel  = FOC_CTRL;
-          rtP_Right.z_ctrlTypSel = 2;
+          rtP_Right.z_ctrlTypSel = FOC_CTRL;
-          ctrlModReqRaw          = 1;
+          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_Left.z_ctrlTypSel  = SIN_CTRL;
-          rtP_Right.z_ctrlTypSel = 1;
+          rtP_Right.z_ctrlTypSel = SIN_CTRL;
           break;
         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;    
       }
       shortBeepMany(sensor1_index + 1);