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:
- 11:03d979f1517f
- Parent:
- 10:f60f9849980a
- Child:
- 12:ff7947a79149
--- a/main.cpp Wed Oct 26 15:27:21 2016 +0000 +++ b/main.cpp Fri Oct 28 10:36:46 2016 +0000 @@ -24,8 +24,8 @@ volatile float q2 = 0; volatile float q1_begin; volatile float q2_begin; -volatile float l1 = 0.3626; -volatile float l2 = 0.420; +const float l1 = 0.3626; +const float l2 = 0.420; volatile float q1_v; volatile float q2_v; volatile float q1_ref = 0; @@ -49,7 +49,8 @@ volatile bool translatie_richting = true; //true is verticaal, false is horizontaal const float TS = 0.02; -const float MotorGain = 8.4; // bij pwm = 1 draait (losse) motor met 8.4 rad/s +const float MotorGain_M1 = 4.3; // bij pwm = 1 draait (losse) motor met 4.3 rad/s -> gemeten +const float MotorGain_M2 = 4.7; // gemeten Ticker update_encoder_ticker; volatile bool go_flag_update_encoder = false; @@ -62,9 +63,9 @@ { //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 \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); + //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,TotalError1,TotalError2); + pc.printf("vx = %f \tvy = %f \tq1_r = %f \tq2_r = %f \tq1 = %f \tq2 = %f \tPID_M1 = %f \tPID_M2 = %f\n\r",vx,vy,q1_ref,q2_ref,q1,q2,ctrlOutput_M1,ctrlOutput_M2); + //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; @@ -97,15 +98,17 @@ { dir_M1 = 0; //ccw dir_M2 = 1; //cw - while (q1 < 20*2*3.1415/360) { + while (q1 < 40*2*3.1415/360) { q1 = wheel_M1.getPulses()/(1334.355/2); - pwm_M1 = 0.01; + pwm_M1 = 0.05; + wait(0.005f); } pwm_M1 = 0; - while (q2 > -45*2*3.1415/360) { + while (q2 > -80*2*3.1415/360) { q2 = wheel_M2.getPulses()/(1334.355/2); - pwm_M2 = 0.01; + pwm_M2 = 0.05; + wait(0.005f); } pwm_M2 = 0; ledg = !ledg; @@ -119,10 +122,10 @@ q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - if (translatie_richting == true) { // verticaal / up + if (translatie_richting == true) { // verticaal / up / blauw vx = 0; vy = 0.1; - } else { // horizontaal / right + } else { // horizontaal / right / rood vx = 0.1; vy = 0; } @@ -134,10 +137,10 @@ q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - if (translatie_richting == true) { // verticaal / down + if (translatie_richting == true) { // verticaal / down / blauw vx = 0; vy = -0.1; - } else { // horizontaal / left + } else { // horizontaal / left / rood vx = -0.1; vy = 0; } @@ -221,17 +224,17 @@ q1IntError = q1IntError + q1_error*TS; // integrated error in radians q2IntError = q2IntError + q2_error*TS; // integrated error in radians - float Ki = 0.1; + float Ki = 1; 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; + float Kd = 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 - motorValue1Out = TotalError1/MotorGain; - motorValue2Out = TotalError2/MotorGain; + motorValue1Out = TotalError1/MotorGain_M1; + motorValue2Out = TotalError2/MotorGain_M2; q1_error_prev = q1_error; q2_error_prev = q2_error; @@ -270,6 +273,9 @@ int main() { + ledr = 1; + ledg = 1; + ledb = 0; pc.baud(115200); wheel_M1.reset(); wheel_M2.reset();