20171016_PID_controller_working
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of 20171013_PI_controller_working_tryingtoaddPID by
main.cpp
- Committer:
- vd
- Date:
- 2017-10-15
- Revision:
- 2:02b385b96543
- Parent:
- 1:b66e14435f70
- Child:
- 3:7e616fcf471d
File content as of revision 2:02b385b96543:
#include "mbed.h" #include "QEI.h" #include "MODSERIAL.h" #include "math.h" DigitalOut gpo(D0); DigitalOut ledb(LED_BLUE); DigitalOut ledr(LED_RED); DigitalOut ledg(LED_GREEN); DigitalOut motor1DC(D7); DigitalOut motor1PWM(D6); DigitalOut motor2DC(D4); DigitalOut motor2PWM(D5); AnalogIn potMeter1(A0); AnalogIn potMeter2(A1); DigitalIn button1(D11); DigitalIn button2(D12); QEI Encoder1(D12,D13,NC,4200); MODSERIAL pc(USBTX,USBRX); Ticker controller; const double pi = 3.1415926535897; 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.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 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(){ double reference = 10*potMeter1.read(); double position = RAD_PER_PULSE*Encoder1.getPulses(); double motor1 = P(reference-position, M1_KP); 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; } } */ // New PID void m1_Controller(){ double reference = 10*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); 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; } } // 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(9600); //controller.attach(&m1_Controller, CONTROLLER_TS); --> P one controller.attach(&m1_Controller, M1_TS); while(1){ 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); //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); } }