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:
- 2:0a976fb06ff8
- Parent:
- 1:e5d2d638eaf4
- Child:
- 3:6ba52d1ae499
--- a/main.cpp Fri Oct 14 09:29:28 2016 +0000 +++ b/main.cpp Mon Oct 17 12:22:29 2016 +0000 @@ -5,74 +5,78 @@ #include "MODSERIAL.h" MODSERIAL pc(USBTX, USBRX); -QEI wheel_M1 (D10, D11, NC, 64); -QEI wheel_M2 (D13, D12, NC, 64); -PwmOut pwm_M1 (D5); -PwmOut pwm_M2 (D6); -DigitalOut dir_M1 (D4); -DigitalOut dir_M2 (D7); +QEI wheel_M1 (D13, D12, NC, 32); +QEI wheel_M2 (D10, D11, NC, 32); +PwmOut pwm_M1 (D6); +PwmOut pwm_M2 (D5); +DigitalOut dir_M1 (D7); +DigitalOut dir_M2 (D4); +DigitalOut ledr (LED_RED); Ticker PIDcontrol; -volatile double q1; -volatile double q2; +volatile double q1 = 0; +volatile double q2 = 0; +volatile double q1_ini = 0; +volatile double q2_ini = 0; volatile double q1_prev = 0; volatile double q2_prev = 0; volatile double pwm_M1_ref = 0; volatile double pwm_M2_ref = 0; volatile double Pulses_M1; volatile double Pulses_M2; +volatile bool go_flag_initialize_M1 = false; -void initialize() +void flag_initialize_M1() { - dir_M1 = 0; //ccw - dir_M2 = 1; //cw - while (q1 > -20*2*3.1415/360) { - Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad - q1 = Pulses_M1 / (1334.355/3); // rad - pwm_M1_ref = 0.1; - } - pwm_M1_ref = 0; + go_flag_initialize_M1 = true; +} - //while (q2 < 45*2*3.1415/360) { - // Pulses_M2 = wheel_M2.getPulses(); - // q2 = Pulses_M2 / (1334.355/3); - // pwm_M2_ref = 0.1; - //} - //pwm_M2_ref = 0; +void initialize_M1() +{ + while (q1_ini < 20*2*3.1415/360) + { + Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad + q1_ini = Pulses_M1 / (1334.355/2); // rad + pwm_M1_ref = 0.1; + //pc.printf("q1_ini = %f\n\r",q1_ini); + } + Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad + q1_ini = Pulses_M1 / (1334.355/2); // rad + //pc.printf("q1_ini = %f\t\tpwm_M1_ref = %f\t\tgo_flag = %B\n\r",q1_ini,pwm_M1_ref,go_flag_initialize_M1); + pwm_M1_ref = 0; + //pc.printf("q1_ini = %f\t\tpwm_M1_ref = %f\t\tgo_flag = %B\n\r",q1_ini,pwm_M1_ref,go_flag_initialize_M1); -const double M1_TS = 0.002; + //while (q2_ini > -45*2*3.1415/360) { + // Pulses_M2 = wheel_M2.getPulses(); + // q2_ini = Pulses_M2 / (1334.355/3); + // pwm_M2_ref = 0.1; + //} + //pwm_M2_ref = 0; +} + +const double M1_TS = 0.2; const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126; -double M1_err_int = 0, n1_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_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 - -//volatile double u; -//volatile double v; -//volatile double v1; -//volatile double v2; -//volatile double y; -//double a1; -//double a2; -//double b0; -//double b1; -//double b2; - -double biquad(double u, double &v1, double &v2, double &v2,const double a1,const double a2,const double b0,const double b1,const double b2); +double biquad_e_der_M1(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; + v2 = v1; + v1=v; return y; } //PID filter -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){ +double PID_M1(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_der = biquad_e_der_M1(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); e_prev = e; // Integral e_int = e_int + Ts*e; @@ -80,17 +84,37 @@ return Kp*e + Ki*e_int + Kd*e_der; } -void M1_controller(){ - double reference = pwm_M1; - q1 = wheel_M1.getPulses() / (1334.355/3); // 1334.355/3 counts/rad - double velocity = (q1 - q1_prev) / 0.002; +void M1_controller() +{ + double reference = pwm_M1_ref; + q1 = wheel_M1.getPulses() / (1334.355/2); // 1334.355/3 counts/rad + double velocity = ((q1 - q1_prev) / M1_TS)/8.3776; // geschaald 0-1 q1_prev = q1; - pwm_M1 = PID(reference - velocity, 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 pwm_M1_PID = PID_M1(reference - velocity, 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); + if (pwm_M1_PID < 0) + { + dir_M1 = 0; + } + else + { + dir_M1 = 1; + } + pwm_M1 = abs(pwm_M1_PID); + pc.printf("q1 = %f,\t\tpwm_M1 = %f, \t\tpwm_M1_ref = %f\t\tpwm_M1_PID = %f \t\te = %f\t\tvelocity = %f\n\r",q1,pwm_M1.read(),pwm_M1_ref,pwm_M1_PID,(reference-velocity),velocity); } int main() { + ledr = 1; pc.baud(115200); - PIDcontrol.attach(&M1_controller, 0.002f); - initialize(); + PIDcontrol.attach(&M1_controller, M1_TS); + flag_initialize_M1(); + dir_M1 = 0; //ccw + dir_M2 = 1; //cw + // initialize function + if (go_flag_initialize_M1 == true) + { + go_flag_initialize_M1 = false; + initialize_M1(); + } } \ No newline at end of file