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