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
parent
22984a7fd6
commit
f2d86f3b30
27
Inc/config.h
27
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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
20
README.md
20
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
|
||||
- **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`.
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
46
Src/main.c
46
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
|
||||
// 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;
|
||||
|
|
|
|||
107
Src/util.c
107
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_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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue