PID controller voor 1 motor die een hoek van 20 graden draait, niet werkend.
Dependencies: MODSERIAL QEI mbed biquadFilter
Inverse Kinematics + PID Controller
main.cpp@2:0a976fb06ff8, 2016-10-17 (annotated)
- Committer:
- willem_hoitzing
- Date:
- Mon Oct 17 12:22:29 2016 +0000
- Revision:
- 2:0a976fb06ff8
- Parent:
- 1:e5d2d638eaf4
- Child:
- 3:6ba52d1ae499
Uitgebreide vorm (instabiel);
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
willem_hoitzing | 0:26ce65a63616 | 1 | #include "stdio.h" |
willem_hoitzing | 0:26ce65a63616 | 2 | #include "math.h" |
willem_hoitzing | 0:26ce65a63616 | 3 | #include "mbed.h" |
willem_hoitzing | 0:26ce65a63616 | 4 | #include "QEI.h" |
willem_hoitzing | 0:26ce65a63616 | 5 | #include "MODSERIAL.h" |
willem_hoitzing | 0:26ce65a63616 | 6 | |
willem_hoitzing | 0:26ce65a63616 | 7 | MODSERIAL pc(USBTX, USBRX); |
willem_hoitzing | 2:0a976fb06ff8 | 8 | QEI wheel_M1 (D13, D12, NC, 32); |
willem_hoitzing | 2:0a976fb06ff8 | 9 | QEI wheel_M2 (D10, D11, NC, 32); |
willem_hoitzing | 2:0a976fb06ff8 | 10 | PwmOut pwm_M1 (D6); |
willem_hoitzing | 2:0a976fb06ff8 | 11 | PwmOut pwm_M2 (D5); |
willem_hoitzing | 2:0a976fb06ff8 | 12 | DigitalOut dir_M1 (D7); |
willem_hoitzing | 2:0a976fb06ff8 | 13 | DigitalOut dir_M2 (D4); |
willem_hoitzing | 2:0a976fb06ff8 | 14 | DigitalOut ledr (LED_RED); |
willem_hoitzing | 0:26ce65a63616 | 15 | |
willem_hoitzing | 0:26ce65a63616 | 16 | Ticker PIDcontrol; |
willem_hoitzing | 0:26ce65a63616 | 17 | |
willem_hoitzing | 2:0a976fb06ff8 | 18 | volatile double q1 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 19 | volatile double q2 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 20 | volatile double q1_ini = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 21 | volatile double q2_ini = 0; |
willem_hoitzing | 0:26ce65a63616 | 22 | volatile double q1_prev = 0; |
willem_hoitzing | 0:26ce65a63616 | 23 | volatile double q2_prev = 0; |
willem_hoitzing | 0:26ce65a63616 | 24 | volatile double pwm_M1_ref = 0; |
willem_hoitzing | 0:26ce65a63616 | 25 | volatile double pwm_M2_ref = 0; |
willem_hoitzing | 0:26ce65a63616 | 26 | volatile double Pulses_M1; |
willem_hoitzing | 0:26ce65a63616 | 27 | volatile double Pulses_M2; |
willem_hoitzing | 2:0a976fb06ff8 | 28 | volatile bool go_flag_initialize_M1 = false; |
willem_hoitzing | 0:26ce65a63616 | 29 | |
willem_hoitzing | 2:0a976fb06ff8 | 30 | void flag_initialize_M1() |
willem_hoitzing | 0:26ce65a63616 | 31 | { |
willem_hoitzing | 2:0a976fb06ff8 | 32 | go_flag_initialize_M1 = true; |
willem_hoitzing | 2:0a976fb06ff8 | 33 | } |
willem_hoitzing | 0:26ce65a63616 | 34 | |
willem_hoitzing | 2:0a976fb06ff8 | 35 | void initialize_M1() |
willem_hoitzing | 2:0a976fb06ff8 | 36 | { |
willem_hoitzing | 2:0a976fb06ff8 | 37 | while (q1_ini < 20*2*3.1415/360) |
willem_hoitzing | 2:0a976fb06ff8 | 38 | { |
willem_hoitzing | 2:0a976fb06ff8 | 39 | Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad |
willem_hoitzing | 2:0a976fb06ff8 | 40 | q1_ini = Pulses_M1 / (1334.355/2); // rad |
willem_hoitzing | 2:0a976fb06ff8 | 41 | pwm_M1_ref = 0.1; |
willem_hoitzing | 2:0a976fb06ff8 | 42 | //pc.printf("q1_ini = %f\n\r",q1_ini); |
willem_hoitzing | 2:0a976fb06ff8 | 43 | } |
willem_hoitzing | 2:0a976fb06ff8 | 44 | Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad |
willem_hoitzing | 2:0a976fb06ff8 | 45 | q1_ini = Pulses_M1 / (1334.355/2); // rad |
willem_hoitzing | 2:0a976fb06ff8 | 46 | //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); |
willem_hoitzing | 2:0a976fb06ff8 | 47 | pwm_M1_ref = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 48 | //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); |
willem_hoitzing | 0:26ce65a63616 | 49 | |
willem_hoitzing | 2:0a976fb06ff8 | 50 | //while (q2_ini > -45*2*3.1415/360) { |
willem_hoitzing | 2:0a976fb06ff8 | 51 | // Pulses_M2 = wheel_M2.getPulses(); |
willem_hoitzing | 2:0a976fb06ff8 | 52 | // q2_ini = Pulses_M2 / (1334.355/3); |
willem_hoitzing | 2:0a976fb06ff8 | 53 | // pwm_M2_ref = 0.1; |
willem_hoitzing | 2:0a976fb06ff8 | 54 | //} |
willem_hoitzing | 2:0a976fb06ff8 | 55 | //pwm_M2_ref = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 56 | } |
willem_hoitzing | 2:0a976fb06ff8 | 57 | |
willem_hoitzing | 2:0a976fb06ff8 | 58 | const double M1_TS = 0.2; |
willem_hoitzing | 0:26ce65a63616 | 59 | const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126; |
willem_hoitzing | 2:0a976fb06ff8 | 60 | double M1_err_int = 0, M1_prev_err = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 61 | 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; |
willem_hoitzing | 1:e5d2d638eaf4 | 62 | double M1_f_v1 = 0, M1_f_v2 = 0; |
willem_hoitzing | 0:26ce65a63616 | 63 | |
willem_hoitzing | 0:26ce65a63616 | 64 | //Biquad filter |
willem_hoitzing | 2:0a976fb06ff8 | 65 | 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) |
willem_hoitzing | 0:26ce65a63616 | 66 | { |
willem_hoitzing | 0:26ce65a63616 | 67 | double v = u - a1*v1 - a2*v2; |
willem_hoitzing | 0:26ce65a63616 | 68 | double y = b0*v + b1*v1 + b2*v2; |
willem_hoitzing | 2:0a976fb06ff8 | 69 | v2 = v1; |
willem_hoitzing | 2:0a976fb06ff8 | 70 | v1=v; |
willem_hoitzing | 0:26ce65a63616 | 71 | return y; |
willem_hoitzing | 0:26ce65a63616 | 72 | } |
willem_hoitzing | 0:26ce65a63616 | 73 | |
willem_hoitzing | 0:26ce65a63616 | 74 | //PID filter |
willem_hoitzing | 2:0a976fb06ff8 | 75 | 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) |
willem_hoitzing | 2:0a976fb06ff8 | 76 | { |
willem_hoitzing | 0:26ce65a63616 | 77 | // Derivative |
willem_hoitzing | 0:26ce65a63616 | 78 | double e_der = (e - e_prev)/Ts; |
willem_hoitzing | 2:0a976fb06ff8 | 79 | e_der = biquad_e_der_M1(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); |
willem_hoitzing | 0:26ce65a63616 | 80 | e_prev = e; |
willem_hoitzing | 0:26ce65a63616 | 81 | // Integral |
willem_hoitzing | 0:26ce65a63616 | 82 | e_int = e_int + Ts*e; |
willem_hoitzing | 0:26ce65a63616 | 83 | // PID |
willem_hoitzing | 0:26ce65a63616 | 84 | return Kp*e + Ki*e_int + Kd*e_der; |
willem_hoitzing | 0:26ce65a63616 | 85 | } |
willem_hoitzing | 0:26ce65a63616 | 86 | |
willem_hoitzing | 2:0a976fb06ff8 | 87 | void M1_controller() |
willem_hoitzing | 2:0a976fb06ff8 | 88 | { |
willem_hoitzing | 2:0a976fb06ff8 | 89 | double reference = pwm_M1_ref; |
willem_hoitzing | 2:0a976fb06ff8 | 90 | q1 = wheel_M1.getPulses() / (1334.355/2); // 1334.355/3 counts/rad |
willem_hoitzing | 2:0a976fb06ff8 | 91 | double velocity = ((q1 - q1_prev) / M1_TS)/8.3776; // geschaald 0-1 |
willem_hoitzing | 0:26ce65a63616 | 92 | q1_prev = q1; |
willem_hoitzing | 2:0a976fb06ff8 | 93 | 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); |
willem_hoitzing | 2:0a976fb06ff8 | 94 | if (pwm_M1_PID < 0) |
willem_hoitzing | 2:0a976fb06ff8 | 95 | { |
willem_hoitzing | 2:0a976fb06ff8 | 96 | dir_M1 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 97 | } |
willem_hoitzing | 2:0a976fb06ff8 | 98 | else |
willem_hoitzing | 2:0a976fb06ff8 | 99 | { |
willem_hoitzing | 2:0a976fb06ff8 | 100 | dir_M1 = 1; |
willem_hoitzing | 2:0a976fb06ff8 | 101 | } |
willem_hoitzing | 2:0a976fb06ff8 | 102 | pwm_M1 = abs(pwm_M1_PID); |
willem_hoitzing | 2:0a976fb06ff8 | 103 | 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); |
willem_hoitzing | 0:26ce65a63616 | 104 | } |
willem_hoitzing | 0:26ce65a63616 | 105 | |
willem_hoitzing | 0:26ce65a63616 | 106 | int main() |
willem_hoitzing | 0:26ce65a63616 | 107 | { |
willem_hoitzing | 2:0a976fb06ff8 | 108 | ledr = 1; |
willem_hoitzing | 0:26ce65a63616 | 109 | pc.baud(115200); |
willem_hoitzing | 2:0a976fb06ff8 | 110 | PIDcontrol.attach(&M1_controller, M1_TS); |
willem_hoitzing | 2:0a976fb06ff8 | 111 | flag_initialize_M1(); |
willem_hoitzing | 2:0a976fb06ff8 | 112 | dir_M1 = 0; //ccw |
willem_hoitzing | 2:0a976fb06ff8 | 113 | dir_M2 = 1; //cw |
willem_hoitzing | 2:0a976fb06ff8 | 114 | // initialize function |
willem_hoitzing | 2:0a976fb06ff8 | 115 | if (go_flag_initialize_M1 == true) |
willem_hoitzing | 2:0a976fb06ff8 | 116 | { |
willem_hoitzing | 2:0a976fb06ff8 | 117 | go_flag_initialize_M1 = false; |
willem_hoitzing | 2:0a976fb06ff8 | 118 | initialize_M1(); |
willem_hoitzing | 2:0a976fb06ff8 | 119 | } |
willem_hoitzing | 0:26ce65a63616 | 120 | } |