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@0:26ce65a63616, 2016-10-14 (annotated)

Committer:
willem_hoitzing
Date:
Fri Oct 14 09:26:14 2016 +0000
Revision:
0:26ce65a63616
Child:
1:e5d2d638eaf4
Foutmelding..

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 0:26ce65a63616 8 QEI wheel_M1 (D10, D11, NC, 64);
willem_hoitzing 0:26ce65a63616 9 QEI wheel_M2 (D13, D12, NC, 64);
willem_hoitzing 0:26ce65a63616 10 PwmOut pwm_M1 (D5);
willem_hoitzing 0:26ce65a63616 11 PwmOut pwm_M2 (D6);
willem_hoitzing 0:26ce65a63616 12 DigitalOut dir_M1 (D4);
willem_hoitzing 0:26ce65a63616 13 DigitalOut dir_M2 (D7);
willem_hoitzing 0:26ce65a63616 14
willem_hoitzing 0:26ce65a63616 15 Ticker PIDcontrol;
willem_hoitzing 0:26ce65a63616 16
willem_hoitzing 0:26ce65a63616 17 volatile double q1;
willem_hoitzing 0:26ce65a63616 18 volatile double q2;
willem_hoitzing 0:26ce65a63616 19 volatile double q1_prev = 0;
willem_hoitzing 0:26ce65a63616 20 volatile double q2_prev = 0;
willem_hoitzing 0:26ce65a63616 21 volatile double pwm_M1_ref = 0;
willem_hoitzing 0:26ce65a63616 22 volatile double pwm_M2_ref = 0;
willem_hoitzing 0:26ce65a63616 23 volatile double Pulses_M1;
willem_hoitzing 0:26ce65a63616 24 volatile double Pulses_M2;
willem_hoitzing 0:26ce65a63616 25
willem_hoitzing 0:26ce65a63616 26 void initialize()
willem_hoitzing 0:26ce65a63616 27 {
willem_hoitzing 0:26ce65a63616 28 dir_M1 = 0; //ccw
willem_hoitzing 0:26ce65a63616 29 dir_M2 = 1; //cw
willem_hoitzing 0:26ce65a63616 30 while (q1 > -20*2*3.1415/360) {
willem_hoitzing 0:26ce65a63616 31 Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad
willem_hoitzing 0:26ce65a63616 32 q1 = Pulses_M1 / (1334.355/3); // rad
willem_hoitzing 0:26ce65a63616 33 pwm_M1_ref = 0.1;
willem_hoitzing 0:26ce65a63616 34 }
willem_hoitzing 0:26ce65a63616 35 pwm_M1_ref = 0;
willem_hoitzing 0:26ce65a63616 36
willem_hoitzing 0:26ce65a63616 37 //while (q2 < 45*2*3.1415/360) {
willem_hoitzing 0:26ce65a63616 38 // Pulses_M2 = wheel_M2.getPulses();
willem_hoitzing 0:26ce65a63616 39 // q2 = Pulses_M2 / (1334.355/3);
willem_hoitzing 0:26ce65a63616 40 // pwm_M2_ref = 0.1;
willem_hoitzing 0:26ce65a63616 41 //}
willem_hoitzing 0:26ce65a63616 42 //pwm_M2_ref = 0;
willem_hoitzing 0:26ce65a63616 43
willem_hoitzing 0:26ce65a63616 44 const double M1_TS = 0.002;
willem_hoitzing 0:26ce65a63616 45 const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126;
willem_hoitzing 0:26ce65a63616 46 double M1_err_int = 0, n1_prev_err = 0;
willem_hoitzing 0:26ce65a63616 47 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 0:26ce65a63616 48 double M2_f_v2 = 0, m2_f_v2 = 0;
willem_hoitzing 0:26ce65a63616 49
willem_hoitzing 0:26ce65a63616 50 //Biquad filter
willem_hoitzing 0:26ce65a63616 51
willem_hoitzing 0:26ce65a63616 52 //volatile double u;
willem_hoitzing 0:26ce65a63616 53 //volatile double v;
willem_hoitzing 0:26ce65a63616 54 //volatile double v1;
willem_hoitzing 0:26ce65a63616 55 //volatile double v2;
willem_hoitzing 0:26ce65a63616 56 //volatile double y;
willem_hoitzing 0:26ce65a63616 57 //double a1;
willem_hoitzing 0:26ce65a63616 58 //double a2;
willem_hoitzing 0:26ce65a63616 59 //double b0;
willem_hoitzing 0:26ce65a63616 60 //double b1;
willem_hoitzing 0:26ce65a63616 61 //double b2;
willem_hoitzing 0:26ce65a63616 62
willem_hoitzing 0:26ce65a63616 63 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);
willem_hoitzing 0:26ce65a63616 64 {
willem_hoitzing 0:26ce65a63616 65 double v = u - a1*v1 - a2*v2;
willem_hoitzing 0:26ce65a63616 66 double y = b0*v + b1*v1 + b2*v2;
willem_hoitzing 0:26ce65a63616 67 v2 = v1;v1=v;
willem_hoitzing 0:26ce65a63616 68 return y;
willem_hoitzing 0:26ce65a63616 69 }
willem_hoitzing 0:26ce65a63616 70
willem_hoitzing 0:26ce65a63616 71 //PID filter
willem_hoitzing 0:26ce65a63616 72 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){
willem_hoitzing 0:26ce65a63616 73 // Derivative
willem_hoitzing 0:26ce65a63616 74 double e_der = (e - e_prev)/Ts;
willem_hoitzing 0:26ce65a63616 75 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
willem_hoitzing 0:26ce65a63616 76 e_prev = e;
willem_hoitzing 0:26ce65a63616 77 // Integral
willem_hoitzing 0:26ce65a63616 78 e_int = e_int + Ts*e;
willem_hoitzing 0:26ce65a63616 79 // PID
willem_hoitzing 0:26ce65a63616 80 return Kp*e + Ki*e_int + Kd*e_der;
willem_hoitzing 0:26ce65a63616 81 }
willem_hoitzing 0:26ce65a63616 82
willem_hoitzing 0:26ce65a63616 83 void M1_controller(){
willem_hoitzing 0:26ce65a63616 84 double reference = pwm_M1;
willem_hoitzing 0:26ce65a63616 85 q1 = wheel_M1.getPulses() / (1334.355/3);
willem_hoitzing 0:26ce65a63616 86 double velocity = (q1 - q1_prev) / 0.002;
willem_hoitzing 0:26ce65a63616 87 q1_prev = q1;
willem_hoitzing 0:26ce65a63616 88 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);
willem_hoitzing 0:26ce65a63616 89 }
willem_hoitzing 0:26ce65a63616 90
willem_hoitzing 0:26ce65a63616 91 int main()
willem_hoitzing 0:26ce65a63616 92 {
willem_hoitzing 0:26ce65a63616 93 pc.baud(115200);
willem_hoitzing 0:26ce65a63616 94 PIDcontrol.attach(&M1_controller, 0.002f);
willem_hoitzing 0:26ce65a63616 95 initialize();
willem_hoitzing 0:26ce65a63616 96 }