Diff: main.cpp

Revision:

3:6ba52d1ae499

Parent:

2:0a976fb06ff8

Child:

4:a5f3e1838e3e

--- a/main.cpp	Mon Oct 17 12:22:29 2016 +0000
+++ b/main.cpp	Thu Oct 20 13:07:52 2016 +0000
@@ -3,6 +3,7 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "MODSERIAL.h"
+#include "BiQuad.h"
 
 MODSERIAL pc(USBTX, USBRX);
 QEI wheel_M1 (D13, D12, NC, 32);
@@ -13,108 +14,98 @@
 DigitalOut dir_M2 (D4);
 DigitalOut ledr (LED_RED);
 
-Ticker PIDcontrol;
-
 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;
+volatile double q1_v_ref = 0;
+volatile double q2_v_ref = 0;
+
+volatile bool go_flag_initialize = false;
 
-void flag_initialize_M1()
+void flag_initialize()
 {
-    go_flag_initialize_M1 = true;
+    go_flag_initialize = true;
+}
+
+void initialize()
+{
+    q1_v_ref = 0.2;     // 0.6 max
+    q2_v_ref = 0.45;
 }
 
-void initialize_M1()
+const double TS = 0.02;
+const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126;
+const double M2_Kp = 4.348, M2_Ki = 36.632, M2_Kd = 0.126;
+const double N = 25;
+
+volatile double velocity_M1;
+volatile double velocity_M2;
+volatile double ctrlOutput_M1 = 0;
+volatile double ctrlOutput_M2 = 0;
+
+Ticker update_velocity_ticker;
+void update_velocity()
 {
-    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);
-
-        //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;
+    double q1 = wheel_M1.getPulses()/(1334.355/2);
+    velocity_M1 = ((q1 - q1_prev) / TS)/5.0265;
+    q1_prev = q1;
+    double q2 = wheel_M2.getPulses()/(1334.355/2);
+    velocity_M2 = ((q2 - q2_prev) / TS)/5.0265;
+    q2_prev = q2;
+    pc.printf("q1_v = %f \tq1_v_ref = %f \tpwm_M1 = %f \tctrlM1 = %f \tq2_v = %f \tq2_v_ref = %f \tpwm_M2 = %f \tctrlM2 = %f\n\r",velocity_M1,q1_v_ref,pwm_M1.read(),ctrlOutput_M1,velocity_M2,q2_v_ref,pwm_M2.read(),ctrlOutput_M2);
 }
 
-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, 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_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;
-    return y;
-}
-
-//PID filter
-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_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;
-    // PID
-    return Kp*e + Ki*e_int + Kd*e_der;
-}
+BiQuad pidf_M1;
+BiQuad pidf_M2;
+Ticker PIDcontrol_M1;
+Ticker PIDcontrol_M2;
 
 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;
-    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)
+    ctrlOutput_M1 = pidf_M1.step(q1_v_ref-velocity_M1);
+    
+    if (ctrlOutput_M1 < 0)
+    {
+        dir_M1 = 1;
+    }
+    else
     {
         dir_M1 = 0;
     }
+    pwm_M1 = abs(ctrlOutput_M1);
+}
+
+void M2_controller()
+{
+    ctrlOutput_M2 = pidf_M2.step(q2_v_ref-velocity_M2);
+    
+    if (ctrlOutput_M2 < 0)
+    {
+        dir_M2 = 1;
+    }
     else
     {
-        dir_M1 = 1;
+        dir_M2 = 0;
     }
-    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);
+    pwm_M2 = abs(ctrlOutput_M2);
 }
 
 int main()
 {
     ledr = 1;
     pc.baud(115200);
-    PIDcontrol.attach(&M1_controller, M1_TS);
-    flag_initialize_M1();
-    dir_M1 = 0; //ccw
-    dir_M2 = 1; //cw
+    pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
+    pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
+    update_velocity_ticker.attach(&update_velocity, 0.02f);
+    PIDcontrol_M1.attach(&M1_controller, TS);
+    PIDcontrol_M2.attach(&M2_controller, TS);
+    flag_initialize();
     // initialize function
-    if (go_flag_initialize_M1 == true) 
+    if (go_flag_initialize == true) 
     {
-        go_flag_initialize_M1 = false;
-        initialize_M1();
+        go_flag_initialize = false;
+        initialize();
     }
+    while(1){}
 }
\ No newline at end of file