Diff: main.cpp

Revision:

4:a5f3e1838e3e

Parent:

3:6ba52d1ae499

Child:

5:0251fde34cdc

--- a/main.cpp	Thu Oct 20 13:07:52 2016 +0000
+++ b/main.cpp	Thu Oct 20 13:51:34 2016 +0000
@@ -16,10 +16,6 @@
 
 volatile double q1 = 0;
 volatile double q2 = 0;
-volatile double q1_prev = 0;
-volatile double q2_prev = 0;
-volatile double q1_v_ref = 0;
-volatile double q2_v_ref = 0;
 
 volatile bool go_flag_initialize = false;
 
@@ -28,32 +24,28 @@
     go_flag_initialize = true;
 }
 
+volatile double q1_ref;
+volatile double q2_ref;
 void initialize()
 {
-    q1_v_ref = 0.2;     // 0.6 max
-    q2_v_ref = 0.45;
+    q1_ref = 2*3.1415 /(2*3.1415);     
+    q2_ref = 3.1415 /(2*3.1415);
 }
 
 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;
+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;
 
-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()
+Ticker update_encoder_ticker;
+void update_encoder()
 {
-    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);
+    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);
 }
 
 BiQuad pidf_M1;
@@ -63,7 +55,7 @@
 
 void M1_controller()
 {
-    ctrlOutput_M1 = pidf_M1.step(q1_v_ref-velocity_M1);
+    ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
     
     if (ctrlOutput_M1 < 0)
     {
@@ -78,7 +70,7 @@
 
 void M2_controller()
 {
-    ctrlOutput_M2 = pidf_M2.step(q2_v_ref-velocity_M2);
+    ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
     
     if (ctrlOutput_M2 < 0)
     {
@@ -97,7 +89,7 @@
     pc.baud(115200);
     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);
+    update_encoder_ticker.attach(&update_encoder, 0.02f);
     PIDcontrol_M1.attach(&M1_controller, TS);
     PIDcontrol_M2.attach(&M2_controller, TS);
     flag_initialize();