Diff: main.cpp

Revision:

6:4d254faf2428

Parent:

5:0251fde34cdc

Child:

7:1444604f10d4

--- a/main.cpp	Mon Oct 24 10:45:05 2016 +0000
+++ b/main.cpp	Mon Oct 24 10:50:17 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;
-volatile double q2_v;
 
 volatile bool go_flag_initialize = false;
 
@@ -28,16 +24,16 @@
     go_flag_initialize = true;
 }
 
-volatile double q1_v_ref;
-volatile double q2_v_ref;
+volatile double q1_ref;
+volatile double q2_ref;
 void initialize()
 {
-    q1_v_ref = 0.4; //2*3.1415 /(2*3.1415);
-    q2_v_ref = 0*3.1415 /(2*3.1415);
+    q1_ref = 0.125; //2*3.1415 /(2*3.1415);
+    q2_ref = 0*3.1415 /(2*3.1415);
 }
 
 const double TS = 0.02;
-const double M1_Kp = 0.05, M1_Ki = 0.00, M1_Kd = 0;
+const double M1_Kp = 1, M1_Ki = 0.00, M1_Kd = 0;
 const double M2_Kp = 0.3, M2_Ki = 0.00, M2_Kd = 0;
 const double N = 0;
 
@@ -55,11 +51,7 @@
 {
     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);
+    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;
@@ -81,32 +73,26 @@
 
 void M1_controller()
 {
-    ctrlOutput_M1 = pidf_M1.step(q1_v_ref-q1_v);
+    ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
 
     if (ctrlOutput_M1 < 0) {
         dir_M1 = 1;
     } else {
         dir_M1 = 0;
     }
-    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;
-    }
+    pwm_M1 = abs(ctrlOutput_M1);
 }
 
 void M2_controller()
 {
-    ctrlOutput_M2 = pidf_M2.step(q2_v_ref-q2_v);
+    ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
 
     if (ctrlOutput_M2 < 0) {
         dir_M2 = 1;
     } else {
         dir_M2 = 0;
     }
-    pwm_M2 = abs(ctrlOutput_M2) + q2_v;
+    pwm_M2 = abs(ctrlOutput_M2);
 }
 
 int main()
@@ -122,7 +108,7 @@
     PIDcontrol_M2.attach(&flag_M2_controller, TS);
     
     // initialize function
-    knop.fall(&initialize);
+    knop.fall(&flag_initialize);
     if (go_flag_initialize == true) {
         go_flag_initialize = false;
         initialize();