Diff: deplacement.cpp

Revision:

97:42167cfeb8d7

Parent:

96:1e91cac784fe

Child:

98:2426d699362b

--- a/deplacement.cpp	Fri May 06 21:21:49 2016 +0000
+++ b/deplacement.cpp	Fri May 06 22:15:43 2016 +0000
@@ -67,6 +67,7 @@
 }
 
 void GotoThet(double theta_) {
+    wait_ms(10);
     roboclaw.ResetEnc();
     wait_ms(10);
     int distance_ticks_left = -theta_*ENTRAXE/(2*(DIAMETRE_ROUE_GAUCHE*3.14159/4096));
@@ -79,19 +80,38 @@
 
     wait(3);
     roboclaw.ForwardM1(0);
+    wait_ms(10);
     roboclaw.ForwardM2(0);
+    wait_ms(10);
 }
 
 void GotoDistPos(double distance)
 {
+    wait_ms(10);
     roboclaw.ResetEnc();
-    wait_ms(1);
+    wait_ms(10);
     int32_t distance_ticks_left = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
     int32_t distance_ticks_right = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
 
     roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
-
-    while(abs(roboclaw.ReadEncM1() - distance_ticks_right) > 20 && abs(roboclaw.ReadEncM2() - distance_ticks_left) > 20);
+    
+    wait_ms(10);
+    int EncM1 = roboclaw.ReadEncM1();
+    wait_ms(10);
+    int EncM2 = roboclaw.ReadEncM2();
+    
+    
+    while(abs(EncM1 - distance_ticks_right) > 20 || abs(EncM2 - distance_ticks_left) > 20)
+    {
+        wait_ms(10);
+        EncM1 = roboclaw.ReadEncM1();
+        wait_ms(10);
+        EncM2 = roboclaw.ReadEncM2();
+    }
     
     wait(0.1);
+    roboclaw.ForwardM1(0);
+    wait_ms(10);
+    roboclaw.ForwardM2(0);
+    wait_ms(10);
 }
\ No newline at end of file