Diff: main.cpp

Revision:

2:6523e21391e5

Parent:

1:078e96685ed3

--- a/main.cpp	Mon Oct 31 14:21:31 2016 +0000
+++ b/main.cpp	Tue Nov 01 09:23:01 2016 +0000
@@ -63,10 +63,12 @@
 volatile bool calibrate_switch = false;
 volatile bool calibration_finished = false;
 
+const double pi = 3.14159265359;
+
 volatile double q1 = 0;
 volatile double q2 = 0;
 const double l1 = 0.3626;
-const double l2 = 0.420;
+const double l2 = (0.420-0.065);        // middelpunt swiffer
 volatile double q1_v;
 volatile double q2_v;
 volatile double q1_ref = 0;
@@ -183,18 +185,22 @@
 {
     dir_M1 = 0; //ccw
     dir_M2 = 1; //cw
-    while (q1 < 20*2*3.1415/360) {
+    while ( (q1 < 20*2*pi/360) || (q2 > -45*2*pi/360) ) {
         q1 = wheel_M1.getPulses()/(1334.355/2);
-        pwm_M1 = 0.05;
+        q2 = wheel_M2.getPulses()/(1334.355/2);
+        if (q1 < 20*2*pi/360) {
+            pwm_M1 = 0.05;
+        } else {
+            pwm_M1 = 0;
+        }
+        if (q2 > -45*2*pi/360) {
+            pwm_M2 = 0.06;
+        } else {
+            pwm_M2 = 0;
+        }
         wait(0.005f);
     }
     pwm_M1 = 0;
-
-    while (q2 > -45*2*3.1415/360) {
-        q2 = wheel_M2.getPulses()/(1334.355/2);
-        pwm_M2 = 0.05;
-        wait(0.005f);
-    }
     pwm_M2 = 0;
     begin_hoeken();
 }
@@ -357,7 +363,7 @@
     q1_v = J_1 * vx + J_2 * vy;
     q2_v = J_3 * vx + J_4 * vy;
 
-    if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
+    if ( (q1 > (135*2*pi/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
         q1_v = 0;
         q2_v = 0;
         q1_ref = q1;
@@ -366,7 +372,7 @@
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
-    } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
+    } else if ( (q1 < -(135*2*pi/360)) && (q1_v < 0) ) {
         q1_v = 0;
         q2_v = 0;
         q1_ref = q1;
@@ -375,7 +381,7 @@
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
-    } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
+    } else if ( (q2 < (-2.6*2*pi/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
         q1_v = 0;
         q2_v = 0;
         q1_ref = q1;
@@ -384,7 +390,7 @@
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
-    } else if ( (q2 > 0) && (q2_v > 0) ) {
+    } else if ( (q2 >= 0) && (q2_v > 0) ) {
         q1_v = 0;
         q2_v = 0;
         q1_ref = q1;