diff --git a/Inc/config.h b/Inc/config.h index 3435575..f4de14f 100644 --- a/Inc/config.h +++ b/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_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) diff --git a/Inc/util.h b/Inc/util.h index a09136a..d0169aa 100644 --- a/Inc/util.h +++ b/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); diff --git a/README.md b/README.md index cf3d175..4d2ff24 100644 --- a/README.md +++ b/README.md @@ -46,10 +46,24 @@ 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. | +(2) | +|FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ | +|FOC TORQUE| +++ | +++ | +++ | ++ | +++(1) | n.a(2) | + +(1) By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter. + +(2) 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) 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. diff --git a/Src/main.c b/Src/main.c index 3bef838..50af650 100644 --- a/Src/main.c +++ b/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,49 +213,51 @@ 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); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); 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 ####### #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; diff --git a/Src/util.c b/Src/util.c index b299e10..9160fc0 100644 --- a/Src/util.c +++ b/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 @@ -460,8 +460,7 @@ void adcCalibLim(void) { ADC1_MIN_temp = MIN(ADC1_MIN_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_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp); - adc_cal_timeout++; + ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp); 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 =========================== */ @@ -731,15 +788,7 @@ void readCommand(void) { timeoutFlagADC = 1; // Timeout detected 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);