Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Anti windup

This commit is contained in:
Stedd 2019-12-24 15:53:26 +01:00
parent 7dffe5208d
commit ec12a38ec6
3 changed files with 63 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Main/Main.ino
View File

@ -65,11 +65,6 @@ void setup() {
m2RawLast = 100; m2RawLast = 100;
// Initialize PWM channels // Initialize PWM channels
// byte pwmPins[4] = {M1_A, M1_B, M2_A, M2_B};
// for(int i = 1; i >= 4; i++){
// ledcAttachPin(pwmPins[i-1], i);
// ledcSetup(i, PWM_CYCLE, PWM_RES);
// }
ledcAttachPin(M1_A, 1); ledcAttachPin(M1_A, 1);
ledcAttachPin(M1_B, 2); ledcAttachPin(M1_B, 2);
ledcAttachPin(M2_A, 3); ledcAttachPin(M2_A, 3);

65
Main/motorControl.ino
View File

@ -13,21 +13,22 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float K_SC = 18.0; //Speed controller gain const float K_SC = 18.5; //Speed controller gain
const float K_TC = 130.0; //Turn controller gain const float K_TC = 90.0; //Turn controller gain
const float K_OL = 14.0; //Outer loop balance controller gain const float K_OL = 13.0; //Outer loop balance controller gain
const float K_IL = 85.0; //Inner loop balance controller gain const float K_IL = 72.0; //Inner loop balance controller gain
const float I_IL = 5.25; //Inner loop balance controller Igain const float I_IL = 80.0; //Inner loop balance controller Igain
const float filter_gain = 16.0; //Motor speed LPF gain const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables //Help variables
int M1_Speed_CMD, M2_Speed_CMD; int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref; float rem_speed_ref, rem_turn_speed_ref;
float ref_SC, act_SC, error_SC, SC_cont_out; float SC_cont_out;
float ref_TC, act_TC, error_TC, TC_cont_out; float TC_cont_out;
float ref_OL, act_OL, error_OL, OL_cont_out; float OL_cont_out;
float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
//Matrices //Matrices
@ -57,27 +58,33 @@ void motors() {
// Remote control commands // Remote control commands
rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2007.0, -3.5, 3.5); if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){
rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25); rem_turn_speed_ref = 0;
rem_speed_ref = 0;
}
else{
rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2015.0, -3.75, 3.75);
rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 2005.0, -0.35, 0.35);
}
// Speed Controller // Speed Controller
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
// Balance controller // Balance controller
// Outer loop // Outer loop
OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
// Inner loop // Inner loop
ref_IL = OL_cont_out; ref_IL = OL_cont_out;
act_IL = pitch_rate; act_IL = pitch_rate;
error_IL = ref_IL - act_IL; error_IL = ref_IL - act_IL;
iError_IL = iError_IL + (error_IL * dT_s * I_IL); iError_IL = iError_IL + (dT_s*(error_IL * I_IL) + (IL_anti_windup*((1/I_IL)+(1/K_IL))));
IL_cont_out = round((error_IL * K_IL) + iError_IL); IL_cont_out = round((error_IL * K_IL) + iError_IL);
//Turn controller //Turn controller
TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC); TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
//Sum speed command for motors //Sum speed command for motors
@ -90,9 +97,9 @@ void motors() {
//Motor control //Motor control
motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
IL_anti_windup = IL_anti_windup/2;
//Update variables for next scan cycle //Update variables for next scan cycle
m1RawLast = m1Raw; m1RawLast = m1Raw;
@ -124,11 +131,12 @@ float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, f
return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_); return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_);
} }
void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) {
//Returns anti windup difference
//Calculate channel //Calculate channel
byte ch2 = motorID * 2; byte ch2 = motorID * 2;
byte ch1 = ch2 - 1; byte ch1 = ch2 - 1;
float windup = 0;
//Deadband //Deadband
if (speedCMD_ > 0 && speedCMD_ < dbPos_) { if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
speedCMD_ = dbPos_; speedCMD_ = dbPos_;
@ -139,9 +147,11 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
// Speed command saturation // Speed command saturation
else if (speedCMD_ > saturation) { else if (speedCMD_ > saturation) {
windup = saturation-speedCMD_;
speedCMD_ = saturation; speedCMD_ = saturation;
} }
else if (speedCMD_ < -saturation) { else if (speedCMD_ < -saturation) {
windup = saturation-speedCMD_;
speedCMD_ = -saturation; speedCMD_ = -saturation;
} }
@ -162,4 +172,7 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
ledcWrite(ch2, 0); ledcWrite(ch2, 0);
} }
return windup;
} }

35
Main/plot.ino
View File

@ -24,10 +24,15 @@ void plot(){
// Remote control // Remote control
// Serial.print("ch1:"); // Serial.print("ch1:");
// Serial.print(remoteCMD[0]); // Serial.print(pwm_time_ch1);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("ch2:"); // Serial.print("ch2:");
// Serial.println(remoteCMD[1]); // Serial.print(pwm_time_ch2);
// Serial.print("ch1mapped:");
// Serial.print(rem_turn_speed_ref);
// Serial.print(" ");
// Serial.print("ch2mapped:");
// Serial.println(rem_speed_ref);
// Encoders // Encoders
@ -38,15 +43,23 @@ void plot(){
// Serial.println(m2Raw); // Serial.println(m2Raw);
// Motors // Motors
// Serial plotter // Serial.print("SpeedControllerOut:");
// Serial.print("Balance_Point:"); // Serial.print(SC_cont_out);
// Serial.print(ref_OL); // Serial.print(" ");
// Serial.print(" "); // Serial.print("BalanceOLControllerOut:");
// Serial.print("Pitch_Angle:"); // Serial.print(OL_cont_out);
// Serial.print(act_OL); // Serial.print(" ");
// Serial.print(" "); // Serial.print("BalanceILControllerOut:");
// Serial.print("Speed_CMD:"); // Serial.print(IL_cont_out);
// Serial.println(Speed_CMD * (100.0 / 4096.0)); // Serial.print(" ");
// Serial.print("TurnControllerOut:");
// Serial.println(TC_cont_out);
// Serial.print(" ");
// Serial.print("M1_CMD:");
// Serial.print(M1_Speed_CMD);
// Serial.print(" ");
// Serial.print("M2_CMD:");
// Serial.println(M2_Speed_CMD);
// Serial.print("M1_Ang_Vel:"); // Serial.print("M1_Ang_Vel:");
// Serial.print(motor_ang_vel[0][0]); // Serial.print(motor_ang_vel[0][0]);