Diff: deplacement.cpp

Revision:

98:2426d699362b

Parent:

97:42167cfeb8d7

Child:

99:1fcb088f8f36

--- a/deplacement.cpp	Fri May 06 22:15:43 2016 +0000
+++ b/deplacement.cpp	Fri May 06 23:55:01 2016 +0000
@@ -1,10 +1,15 @@
 #include "entete.h"
 
-void GotoDist(float timer) {
+int max(int a, int b)
+{
+    return (a<b)?b:a;
+}
+
+void GotoDist(float timer, int acc, int speed, int decc) {
     Timer t;
     
-    roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
-    roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+    roboclaw.SpeedAccelM1(acc, speed);
+    roboclaw.SpeedAccelM2(acc, speed);
     
     t.reset();
     t.start();
@@ -17,8 +22,8 @@
             t.stop();
             while (Ravance != 1);
             wait_ms(1);
-            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
-            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+            roboclaw.SpeedAccelM1(acc, speed);
+            roboclaw.SpeedAccelM2(acc, speed);
             t.start();
         }
     }
@@ -32,29 +37,17 @@
     wait(waiting_time);
 }
 
-void GotoArr(float timer) {
+void GotoArr(float timer, int acc, int speed, int decc)  {
     Timer t;
     
-    roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
-    roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
+    roboclaw.SpeedAccelM1(acc, -speed);
+    roboclaw.SpeedAccelM2(acc, -speed);
     wait_ms(1);
     
     t.reset();
     t.start();
     
-    while (t.read() < timer) {
-        if (Ravance != 1) {
-            roboclaw.ForwardM1(0);
-            wait_ms(1);
-            roboclaw.ForwardM2(0);
-            t.stop();
-            while (Ravance != 1);
-            wait_ms(1);
-            roboclaw.SpeedAccelM1(accel_dista, -vitesse_dista);
-            roboclaw.SpeedAccelM2(accel_dista, -vitesse_dista);
-            t.start();
-        }
-    }
+    while (t.read() < timer);
     
     roboclaw.ForwardM1(0);
     wait_ms(1);
@@ -72,13 +65,13 @@
     wait_ms(10);
     int distance_ticks_left = -theta_*ENTRAXE/(2*(DIAMETRE_ROUE_GAUCHE*3.14159/4096));
     int distance_ticks_right = theta_*ENTRAXE/(2*(DIAMETRE_ROUE_DROITE*3.14159/4096));
-
-    if (theta_ < 0)
-        roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
-    else
-        roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
 
     wait(3);
+    wait_ms(10);
+    roboclaw.ResetEnc();
+    wait_ms(10);
     roboclaw.ForwardM1(0);
     wait_ms(10);
     roboclaw.ForwardM2(0);
@@ -97,19 +90,36 @@
     
     wait_ms(10);
     int EncM1 = roboclaw.ReadEncM1();
+    int EncM1_t = 0;
     wait_ms(10);
     int EncM2 = roboclaw.ReadEncM2();
+    int EncM2_t = 0;
+    
+    int speed = 500;
+    
+    Timer t;
+    
+    t.start();
     
     
-    while(abs(EncM1 - distance_ticks_right) > 20 || abs(EncM2 - distance_ticks_left) > 20)
+    while((abs(EncM1 - distance_ticks_right) > 20 || abs(EncM2 - distance_ticks_left) > 20) && (speed > 2 || t.read() < 0.5f))
     {
-        wait_ms(10);
+        wait_ms(200);
         EncM1 = roboclaw.ReadEncM1();
         wait_ms(10);
         EncM2 = roboclaw.ReadEncM2();
+        
+        speed = abs(EncM1 - EncM1_t);
+        speed = max(abs(EncM2 - EncM2_t),speed);
+        EncM1_t = EncM1;
+        EncM2_t = EncM2;
     }
     
     wait(0.1);
+    
+    wait_ms(10);
+    roboclaw.ResetEnc();
+    wait_ms(10);
     roboclaw.ForwardM1(0);
     wait_ms(10);
     roboclaw.ForwardM2(0);