![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Working P loop
Dependencies: MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 2:c2c76a7a7250
- Parent:
- 1:b66e14435f70
diff -r b66e14435f70 -r c2c76a7a7250 main.cpp --- a/main.cpp Wed Oct 11 09:11:20 2017 +0000 +++ b/main.cpp Fri Oct 13 08:00:35 2017 +0000 @@ -25,13 +25,23 @@ Ticker controller; const double pi = 3.1415926535897; -const float M1_KP = 10, M1_KI = 0.0; -const double M1_TS = 0.0001; +const float M1_KP = 20 , M1_KI = 1.0, M1_KD = 0.5; +const double M1_TS = 0.001; const float RAD_PER_PULSE = (2*pi)/4200; -const float CONTROLLER_TS = 0.0001; //TIME INTERVAL/ hZ -double m1_err_int = 0; +const float CONTROLLER_TS = 0.001; //TIME INTERVAL/ hZ +double m1_err_int = 0, m1_prev_err = 0; +const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0; +double m1_f_v1=0, m1_f_v2 = 0; -// the working P controller beneath here +//Biquad filter +double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){ + double v = u-a1*v1-a2*v2; + double y = b0*v+b1*v1+b2*v2; + v2 = v1;v1 = v; + return y; +} + +/* the working P controller beneath here //float P(double error, const float Kp){ // return Kp * error; // } @@ -41,6 +51,19 @@ double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){ e_int += Ts*e; return Kp*e+Ki*e_int; +}*/ + +//new PID part + +double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){ +// Derivative + double e_der = (e-e_prev)/Ts; + e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 ); + e_prev = e; +// Integral + e_int += Ts*e; +// PID + return Kp * e + Ki * e_int + Kd * e_der; } /* Working P controller part @@ -78,7 +101,7 @@ } */ -// New PI part +/*/ New PI part void m1_Controller(){ double reference = 10*potMeter1.read(); @@ -112,6 +135,41 @@ } +*/ + +void m1_Controller() { + double reference = potMeter1.read(); + double position = RAD_PER_PULSE*Encoder1.getPulses(); + //float motor1 = PID( reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2 ); + //pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position); + pc.printf("\r value motor1: . reference Pot: %f. Position: %f \n", reference, position); + + motor1PWM = motor1; + + if(motor1 > 0.5){ + motor1DC = 1; + + ledr = 1; + ledg = 1; //Blau + ledb = 0; + } + else if (motor1<-0.5) { + motor1DC = 0; + + ledb = 1; + ledr = 1; + ledg = 0; //Groen + + } + else{ + motor1PWM = 0; + + ledb = 1; //Rood + ledr = 0; + ledg = 1; + } + +} int main(){ pc.baud(115200); @@ -123,8 +181,8 @@ double reference = 10*potMeter1.read(); double position = RAD_PER_PULSE*Encoder1.getPulses(); // double motor1 = P(reference-position, M1_KP); --> old one - float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int); - - pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position); + //float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int); + float motor1 = PID( reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2 ); + } }