Diff: main.cpp

Revision:

8:008a7bf80fa0

Parent:

7:1444604f10d4

Child:

9:334b1596637b

--- a/main.cpp	Tue Oct 25 13:03:24 2016 +0000
+++ b/main.cpp	Wed Oct 26 07:59:23 2016 +0000
@@ -37,8 +37,8 @@
 volatile bool translatie_richting = true;  //true is verticaal, false is horizontaal
 
 const double TS = 0.02;
-const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0.1;        // Waardes vinden?
-const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0.1;
+const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0;        // Waardes vinden?
+const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0;
 const double N = 0.1;
 
 Ticker begin_waarde_q;
@@ -148,8 +148,9 @@
     q1_v = J_1 * vx + J_2 * vy;
     q2_v = J_3 * vx + J_4 * vy;
 
-    q1_ref = q1_ref + q1_v*TS + q1_begin;
-    q2_ref = q2_ref + q2_v*TS + q2_begin;
+    q1_ref = q1 + q1_v*TS;
+    q2_ref = q2 + q2_v*TS;
+    
     if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
         q1_v = 0;
         q2_v = 0;
@@ -192,6 +193,11 @@
         dir_M1 = 0;
     }
     pwm_M1 = abs(ctrlOutput_M1);
+    if (pwm_M1 < 0) {
+        pwm_M1 = 0;
+    //} else {
+    //    pwm_M1 = pwm_M1 + 0.1;
+    }
 }
 
 void M2_controller()
@@ -204,6 +210,11 @@
         dir_M2 = 0;
     }
     pwm_M2 = abs(ctrlOutput_M2);
+    if (pwm_M2 < 0) {
+        pwm_M2 = 0;
+    //} else {
+    //    pwm_M2 = pwm_M2 + 0.1;
+    }
 }
 
 int main()
@@ -224,6 +235,9 @@
     PIDcontrol_M2.attach(&flag_M2_controller, TS);
     begin_waarde_q.attach(&begin_waarde, 3);
     
+    pc.printf("pidf_M1 is %s", pidf_M1.stable() ? "stable" : "unstable");
+    pc.printf("pidf_M2 is %s", pidf_M2.stable() ? "stable" : "unstable");
+    
     while(1) {
 
         // initialize function