From ec12a38ec6cf8a1375a3e8c835566eaac88a0f10 Mon Sep 17 00:00:00 2001 From: Stedd Date: Tue, 24 Dec 2019 15:53:26 +0100 Subject: [PATCH] Anti windup --- Main/Main.ino | 5 ---- Main/motorControl.ino | 65 ++++++++++++++++++++++++++----------------- Main/plot.ino | 35 +++++++++++++++-------- 3 files changed, 63 insertions(+), 42 deletions(-) diff --git a/Main/Main.ino b/Main/Main.ino index 3f10e99..b4dd646 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -65,11 +65,6 @@ void setup() { m2RawLast = 100; // 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_B, 2); ledcAttachPin(M2_A, 3); diff --git a/Main/motorControl.ino b/Main/motorControl.ino index 417f26c..e753074 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -13,21 +13,22 @@ const float DEADBAND_M2_NEG = 90.0; //Tuning -const float K_SC = 18.0; //Speed controller gain -const float K_TC = 130.0; //Turn controller gain -const float K_OL = 14.0; //Outer loop balance controller gain -const float K_IL = 85.0; //Inner loop balance controller gain -const float I_IL = 5.25; //Inner loop balance controller Igain +const float K_SC = 18.5; //Speed controller gain +const float K_TC = 90.0; //Turn controller gain +const float K_OL = 13.0; //Outer loop balance controller gain +const float K_IL = 72.0; //Inner loop balance controller gain +const float I_IL = 80.0; //Inner loop balance controller Igain const float filter_gain = 16.0; //Motor speed LPF gain //Help variables int M1_Speed_CMD, M2_Speed_CMD; float rem_speed_ref, rem_turn_speed_ref; -float ref_SC, act_SC, error_SC, SC_cont_out; -float ref_TC, act_TC, error_TC, TC_cont_out; -float ref_OL, act_OL, error_OL, OL_cont_out; -float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL; +float SC_cont_out; +float TC_cont_out; +float OL_cont_out; +float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; + //Matrices @@ -57,27 +58,33 @@ void motors() { // Remote control commands - rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2007.0, -3.5, 3.5); - rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25); + if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){ + 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 - 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 // 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 - ref_IL = OL_cont_out; - act_IL = pitch_rate; - error_IL = ref_IL - act_IL; - iError_IL = iError_IL + (error_IL * dT_s * I_IL); - IL_cont_out = round((error_IL * K_IL) + iError_IL); + ref_IL = OL_cont_out; + act_IL = pitch_rate; + error_IL = ref_IL - act_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); //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 @@ -90,9 +97,9 @@ void motors() { //Motor control - 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 = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_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 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_); } -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 - byte ch2 = motorID * 2; - byte ch1 = ch2 - 1; - + byte ch2 = motorID * 2; + byte ch1 = ch2 - 1; + float windup = 0; //Deadband if (speedCMD_ > 0 && speedCMD_ < dbPos_) { speedCMD_ = dbPos_; @@ -139,9 +147,11 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo // Speed command saturation else if (speedCMD_ > saturation) { + windup = saturation-speedCMD_; speedCMD_ = saturation; } else if (speedCMD_ < -saturation) { + windup = saturation-speedCMD_; speedCMD_ = -saturation; } @@ -162,4 +172,7 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo ledcWrite(ch1, 0); ledcWrite(ch2, 0); } + + return windup; + } diff --git a/Main/plot.ino b/Main/plot.ino index 8f58c75..c0ea6b0 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -24,10 +24,15 @@ void plot(){ // Remote control // Serial.print("ch1:"); - // Serial.print(remoteCMD[0]); + // Serial.print(pwm_time_ch1); // Serial.print(" "); // 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 @@ -38,15 +43,23 @@ void plot(){ // Serial.println(m2Raw); // Motors - // Serial plotter - // Serial.print("Balance_Point:"); - // Serial.print(ref_OL); - // Serial.print(" "); - // Serial.print("Pitch_Angle:"); - // Serial.print(act_OL); - // Serial.print(" "); - // Serial.print("Speed_CMD:"); - // Serial.println(Speed_CMD * (100.0 / 4096.0)); + // Serial.print("SpeedControllerOut:"); + // Serial.print(SC_cont_out); + // Serial.print(" "); + // Serial.print("BalanceOLControllerOut:"); + // Serial.print(OL_cont_out); + // Serial.print(" "); + // Serial.print("BalanceILControllerOut:"); + // Serial.print(IL_cont_out); + // 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(motor_ang_vel[0][0]);