PIDcontroller_without_biquadfilter
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of 20170910_PID_V1_0 by
Revision 2:02b385b96543, committed 2017-10-15
- Comitter:
- vd
- Date:
- Sun Oct 15 21:19:11 2017 +0000
- Parent:
- 1:b66e14435f70
- Commit message:
- PIDcontroller_without_biquadfilter
Changed in this revision
biquadFilter.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r b66e14435f70 -r 02b385b96543 biquadFilter.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/biquadFilter.lib Sun Oct 15 21:19:11 2017 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
diff -r b66e14435f70 -r 02b385b96543 main.cpp --- a/main.cpp Wed Oct 11 09:11:20 2017 +0000 +++ b/main.cpp Sun Oct 15 21:19:11 2017 +0000 @@ -3,8 +3,6 @@ #include "MODSERIAL.h" #include "math.h" - - DigitalOut gpo(D0); DigitalOut ledb(LED_BLUE); DigitalOut ledr(LED_RED); @@ -25,24 +23,78 @@ Ticker controller; const double pi = 3.1415926535897; -const float M1_KP = 10, M1_KI = 0.0; -const double M1_TS = 0.0001; +const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. + +//verplaatst 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.01; //TIME INTERVAL/ hZ + +const float M1_KP = 10, M1_KI = 1.0, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5 +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 ; + + +/*/ 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; // v=v1 vervangen +v2 = v1; v1=v; +return y; +} +*/ + +/* +const float RAD_PER_PULSE = (2*pi)/4200; +const float CONTROLLER_TS = 0.001; //TIME INTERVAL/ hZ + +*/ + +//double m1_err_int = 0; --> PI + + // the working P controller beneath here //float P(double error, const float Kp){ // return Kp * error; // } -// New PI controller +//New PID +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; + + +// biquad part, see slide +//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 PI controller 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; } +*/ + + /* Working P controller part void motor1_Controller(){ @@ -78,23 +130,22 @@ } */ -// New PI part +// New PID void m1_Controller(){ double reference = 10*potMeter1.read(); double position = RAD_PER_PULSE*Encoder1.getPulses(); - 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); motor1PWM = motor1; - if(motor1 > 0.5){ + if(motor1 > 0.1){ motor1DC = 1; ledr = 1; ledg = 1; //Blau ledb = 0; } - else if (motor1<-0.5) { + else if (motor1< -0.1) { motor1DC = 0; ledb = 1; @@ -113,8 +164,45 @@ } + +// New PI part +/* +void m1_Controller(){ + double reference = 10*potMeter1.read(); + double position = RAD_PER_PULSE*Encoder1.getPulses(); + float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int); + motor1PWM = motor1; + + if(motor1 > 0.1){ + motor1DC = 1; + + ledr = 1; + ledg = 1; //Blau + ledb = 0; + } + else if (motor1< -0.1) { + motor1DC = 0; + + ledb = 1; + ledr = 1; + ledg = 0; //Groen + + } + else{ + motor1PWM = 0; + + ledb = 1; //Rood + ledr = 0; + ledg = 1; + } + + +} + +*/ + int main(){ - pc.baud(115200); + pc.baud(9600); //controller.attach(&m1_Controller, CONTROLLER_TS); --> P one controller.attach(&m1_Controller, M1_TS); @@ -123,8 +211,11 @@ 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); + double 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); } } +