Willem Hoitzing / Mbed 2 deprecated PID_controller

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?

UserRevisionLine numberNew 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 }