PID controller voor 1 motor die een hoek van 20 graden draait, niet werkend.
Dependencies: MODSERIAL QEI mbed biquadFilter
Inverse Kinematics + PID Controller
Diff: main.cpp
- Revision:
- 10:f60f9849980a
- Parent:
- 9:334b1596637b
- Child:
- 11:03d979f1517f
--- a/main.cpp Wed Oct 26 14:00:22 2016 +0000 +++ b/main.cpp Wed Oct 26 15:27:21 2016 +0000 @@ -28,20 +28,20 @@ volatile float l2 = 0.420; volatile float q1_v; volatile float q2_v; - float q1_ref = 0; - float q2_ref = 0; - float q1_error = 0; - float q2_error = 0; - float q1_error_prev = 0; - float q2_error_prev = 0; - float q1DerivativeError = 0; - float q2DerivativeError = 0; - float q1IntError = 0; - float q2IntError = 0; - float TotalError1= 0; - float TotalError2= 0; - float motorValue1Out = 0.0; - float motorValue2Out = 0.0; +volatile float q1_ref = 0; +volatile float q2_ref = 0; +volatile float q1_error = 0; +volatile float q2_error = 0; +volatile float q1_error_prev = 0; +volatile float q2_error_prev = 0; +volatile float q1DerivativeError = 0; +volatile float q2DerivativeError = 0; +volatile float q1IntError = 0; +volatile float q2IntError = 0; +volatile float TotalError1= 0; +volatile float TotalError2= 0; +float motorValue1Out = 0.0; +float motorValue2Out = 0.0; volatile float ctrlOutput_M1 = 0; volatile float ctrlOutput_M2 = 0; volatile float vx; @@ -60,10 +60,11 @@ void update_encoder() { - q1 = wheel_M1.getPulses()/(1334.355/2); - q2 = wheel_M2.getPulses()/(1334.355/2); + //q1 = wheel_M1.getPulses()/(1334.355/2); + //q2 = wheel_M2.getPulses()/(1334.355/2); //pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \ttotalerror1 = %f \ttotalerror2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,TotalError1,TotalError2); - pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f\n\r",vx,vy,q1_v,q2_v); + //pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f \tq1 = %f \tq2 = %f \tpwm_M1 = %f \tpwm_M2 = %f\n\r",vx,vy,q1_v,q2_v,q1,q2,pwm_M1.read(),pwm_M2.read()); + pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,TotalError1,TotalError2); } volatile bool go_flag_initialize = false; @@ -86,8 +87,8 @@ void begin_hoeken() { wait(3); - float q1_ref = wheel_M1.getPulses()/(1334.355/2); - float q2_ref = wheel_M2.getPulses()/(1334.355/2); + q1_ref = wheel_M1.getPulses()/(1334.355/2); + q2_ref = wheel_M2.getPulses()/(1334.355/2); active_PID_ticker = true; } @@ -114,22 +115,30 @@ void biceps() { - if (translatie_richting) { // verticaal / up + q1IntError = 0; + q2IntError = 0; + q1_error_prev = 0; + q2_error_prev = 0; + if (translatie_richting == true) { // verticaal / up vx = 0; - vy = 0.02; + vy = 0.1; } else { // horizontaal / right - vx = 0.02; + vx = 0.1; vy = 0; } } void triceps() { - if (translatie_richting) { // verticaal / down + q1IntError = 0; + q2IntError = 0; + q1_error_prev = 0; + q2_error_prev = 0; + if (translatie_richting == true) { // verticaal / down vx = 0; - vy = -0.02; + vy = -0.1; } else { // horizontaal / left - vx = -0.02; + vx = -0.1; vy = 0; } @@ -137,11 +146,17 @@ void switcher() { - if ( (vx == 0) && (vy == 0) ) { - translatie_richting = !translatie_richting; + if ( (vx == 0) && (vy == 0) && (translatie_richting == true) ) { + translatie_richting = false; + } else if ( (vx == 0) && (vy == 0) && (translatie_richting == false) ) { + translatie_richting = true; } else { vx = 0; vy = 0; + q1IntError = 0; + q2IntError = 0; + q1_error_prev = 0; + q2_error_prev = 0; } if (translatie_richting == 1) { @@ -192,7 +207,7 @@ q1_v = 0; q2_v = 0; } - + q1_ref = q1 + q1_v*TS; q2_ref = q2 + q2_v*TS; } @@ -200,37 +215,24 @@ void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out) { // linear feedback control - float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians - float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians - float Kp = 1; + q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians + q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians + float Kp = 10; - float q1IntError = q1IntError + q1_error*TS; // integrated error in radians - float q2IntError = q2IntError + q2_error*TS; // integrated error in radians + q1IntError = q1IntError + q1_error*TS; // integrated error in radians + q2IntError = q2IntError + q2_error*TS; // integrated error in radians float Ki = 0.1; - - float q1DerivativeError = (q1_error - q1_error_prev)/TS; // derivative of error in radians - float q2DerivativeError = (q2_error - q2_error_prev)/TS; // derivative of error in radians + + q1DerivativeError = (q1_error - q1_error_prev)/TS; // derivative of error in radians + q2DerivativeError = (q2_error - q2_error_prev)/TS; // derivative of error in radians float Kd = 0.0; - - TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output = motor input - TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output = motor input - - if (TotalError1 < 0) { - TotalError1=0; - } - else { - TotalError1=TotalError1; - } - if (TotalError2 < 0) { - TotalError2=0; - } - else { - TotalError2=TotalError2; - } - - motorValue1Out = TotalError1/MotorGain; + + TotalError1 = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd); //total controller output = motor input + TotalError2 = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd); //total controller output = motor input + + motorValue1Out = TotalError1/MotorGain; motorValue2Out = TotalError2/MotorGain; - + q1_error_prev = q1_error; q2_error_prev = q2_error; } @@ -240,7 +242,7 @@ PID(q1,q1_ref,q2,q2_ref,TS,motorValue1Out,motorValue2Out); ctrlOutput_M1 = motorValue1Out; ctrlOutput_M2 = motorValue2Out; - + if (ctrlOutput_M1 < 0) { dir_M1 = 1; } else { @@ -274,19 +276,19 @@ knop_biceps.rise(&biceps); knop_triceps.rise(&triceps); knop_switch.rise(&switcher); - + // flag functions/tickers update_encoder_ticker.attach(&flag_update_encoder, TS); update_ref_ticker.attach(&flag_update_ref, TS); // initialize -> beginposities initialize(); - + if (active_PID_ticker == true) { - PIDcontrol.attach(&flag_controller, TS); + PIDcontrol.attach(&flag_controller, TS); } - + while(1) { - + // update encoder if (go_flag_update_encoder == true) { go_flag_update_encoder = false;