Diff: main.cpp

Revision:

5:0251fde34cdc

Parent:

4:a5f3e1838e3e

Child:

6:4d254faf2428

--- a/main.cpp	Thu Oct 20 13:51:34 2016 +0000
+++ b/main.cpp	Mon Oct 24 10:45:05 2016 +0000
@@ -12,10 +12,14 @@
 PwmOut pwm_M2 (D5);
 DigitalOut dir_M1 (D7);
 DigitalOut dir_M2 (D4);
-DigitalOut ledr (LED_RED);
+InterruptIn knop(SW3);
 
 volatile double q1 = 0;
 volatile double q2 = 0;
+volatile double q1_prev = 0;
+volatile double q2_prev = 0;
+volatile double q1_v;
+volatile double q2_v;
 
 volatile bool go_flag_initialize = false;
 
@@ -24,80 +28,121 @@
     go_flag_initialize = true;
 }
 
-volatile double q1_ref;
-volatile double q2_ref;
+volatile double q1_v_ref;
+volatile double q2_v_ref;
 void initialize()
 {
-    q1_ref = 2*3.1415 /(2*3.1415);     
-    q2_ref = 3.1415 /(2*3.1415);
+    q1_v_ref = 0.4; //2*3.1415 /(2*3.1415);
+    q2_v_ref = 0*3.1415 /(2*3.1415);
 }
 
 const double TS = 0.02;
-const double M1_Kp = 0.4086, M1_Ki = 0.0046, M1_Kd = 3.2710;
-const double M2_Kp = 0.4086, M2_Ki = 0.0046, M2_Kd = 3.2710;
-const double N = 3.0624;
+const double M1_Kp = 0.05, M1_Ki = 0.00, M1_Kd = 0;
+const double M2_Kp = 0.3, M2_Ki = 0.00, M2_Kd = 0;
+const double N = 0;
 
 volatile double ctrlOutput_M1 = 0;
 volatile double ctrlOutput_M2 = 0;
 
 Ticker update_encoder_ticker;
+volatile bool go_flag_update_encoder = false;
+void flag_update_encoder()
+{
+    go_flag_update_encoder = true;
+}
+
 void update_encoder()
 {
-    q1 = wheel_M1.getPulses()/(1334.355/2) /(2*3.1415);
-    q2 = wheel_M2.getPulses()/(1334.355/2) /(2*3.1415);
-    pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2);
+    q1 = wheel_M1.getPulses()/(1334.355/2);
+    q2 = wheel_M2.getPulses()/(1334.355/2);
+    q1_v = (q1-q1_prev)/0.02/0.3;
+    q2_v = (q2-q2_prev)/0.02/0.3;
+    q1_prev = q1;
+    q2_prev = q2;
+    pc.printf("q1_v = %f \tq1_v_ref = %f \tPID1 = %f \tq2_v = %f \tq2_v_ref = %f \tPID2 = %f\n\r",q1_v, q1_v_ref, ctrlOutput_M1,q2_v,q2_v_ref,ctrlOutput_M2);
 }
 
 BiQuad pidf_M1;
 BiQuad pidf_M2;
 Ticker PIDcontrol_M1;
 Ticker PIDcontrol_M2;
+volatile bool go_flag_M1_controller = false;
+volatile bool go_flag_M2_controller = false;
+
+void flag_M1_controller()
+{
+    go_flag_M1_controller = true;
+}
+
+void flag_M2_controller()
+{
+    go_flag_M2_controller = true;
+}
 
 void M1_controller()
 {
-    ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
-    
-    if (ctrlOutput_M1 < 0)
-    {
+    ctrlOutput_M1 = pidf_M1.step(q1_v_ref-q1_v);
+
+    if (ctrlOutput_M1 < 0) {
         dir_M1 = 1;
-    }
-    else
-    {
+    } else {
         dir_M1 = 0;
     }
-    pwm_M1 = abs(ctrlOutput_M1);
+    if (q1>0.5*3.1415) {
+        pwm_M1 = 0;
+    } else if (q1<-0.5*3.1415) {
+        pwm_M1 = 0;
+    } else {
+    pwm_M1 = abs(ctrlOutput_M1) + q1_v;
+    }
 }
 
 void M2_controller()
 {
-    ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
-    
-    if (ctrlOutput_M2 < 0)
-    {
+    ctrlOutput_M2 = pidf_M2.step(q2_v_ref-q2_v);
+
+    if (ctrlOutput_M2 < 0) {
         dir_M2 = 1;
-    }
-    else
-    {
+    } else {
         dir_M2 = 0;
     }
-    pwm_M2 = abs(ctrlOutput_M2);
+    pwm_M2 = abs(ctrlOutput_M2) + q2_v;
 }
 
 int main()
 {
-    ledr = 1;
     pc.baud(115200);
+    wheel_M1.reset();
+    wheel_M2.reset();
     pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
     pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
-    update_encoder_ticker.attach(&update_encoder, 0.02f);
-    PIDcontrol_M1.attach(&M1_controller, TS);
-    PIDcontrol_M2.attach(&M2_controller, TS);
-    flag_initialize();
+    // flag functions/tickers
+    update_encoder_ticker.attach(&flag_update_encoder, 0.02f);
+    PIDcontrol_M1.attach(&flag_M1_controller, TS);
+    PIDcontrol_M2.attach(&flag_M2_controller, TS);
+    
     // initialize function
-    if (go_flag_initialize == true) 
-    {
+    knop.fall(&initialize);
+    if (go_flag_initialize == true) {
         go_flag_initialize = false;
         initialize();
     }
-    while(1){}
+    
+    while(1) {
+        // update encoder
+        if (go_flag_update_encoder == true) {
+            go_flag_update_encoder = false;
+            update_encoder();
+        }
+        // controller M1
+        if (go_flag_M1_controller == true) {
+            go_flag_M1_controller = false;
+            M1_controller();
+        }        
+        // controller M2
+        if (go_flag_M2_controller == true) {
+            go_flag_M2_controller = false;
+            M2_controller();
+        }
+    }
 }
\ No newline at end of file