Revision 98:2426d699362b, committed 2016-05-06

Comitter:

Jagang

Date:

Fri May 06 23:55:01 2016 +0000

Parent:

97:42167cfeb8d7

Child:

99:1fcb088f8f36

Commit message:

petit push ? romain;

Changed in this revision

--- 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);

--- a/entete.h	Fri May 06 22:15:43 2016 +0000
+++ b/entete.h	Fri May 06 23:55:01 2016 +0000
@@ -6,6 +6,26 @@
 
 #define PI 3.14159f
 
+#define ENTRAXE 243.8
+#define DIAMETRE_ROUE_GAUCHE 61.7
+#define DIAMETRE_ROUE_DROITE 61.8
+
+#define accel_angle 3200
+#define vitesse_angle 3200
+#define deccel_angle 3200
+
+#define accel_dista 4000
+#define vitesse_dista 4000
+#define deccel_dista 4000
+
+#define waiting_time 1
+#define GAUCHE 1
+#define DROITE -1
+
+#define VERT 1
+#define VIOLET 2
+#define NOIR 3
+
 /* Classes du projet : 
 AX12
 Roboclaw
@@ -38,8 +58,8 @@
 void init_globals();
 
 //vFonctions deplacement.cpp
-void GotoDist (float timer);
-void GotoArr(float timer);
+void GotoDist (float timer, int acc=accel_dista, int speed=vitesse_dista, int decc=deccel_dista);
+void GotoArr(float timer, int acc=accel_dista, int speed=vitesse_dista, int decc=deccel_dista);
 void GotoThet(double theta_);
 void GotoDistPos(double distance);
 
@@ -49,24 +69,4 @@
 void depart(void);
 void changeCamp(void);
 
-#define ENTRAXE 243.8
-#define DIAMETRE_ROUE_GAUCHE 61.7
-#define DIAMETRE_ROUE_DROITE 61.8
-
-#define accel_angle 3200
-#define vitesse_angle 3200
-#define deccel_angle 3200
-
-#define accel_dista 4000
-#define vitesse_dista 4000
-#define deccel_dista 4000
-
-#define waiting_time 1
-#define GAUCHE 1
-#define DROITE -1
-
-#define VERT 1
-#define VIOLET 2
-#define NOIR 3
-
 #endif
\ No newline at end of file

--- a/main.cpp	Fri May 06 22:15:43 2016 +0000
+++ b/main.cpp	Fri May 06 23:55:01 2016 +0000
@@ -54,54 +54,55 @@
         GotoArr(9.2);
         R_SEUIL_SHARP = 0.25;
         GotoDist(2.5);
-        GotoThet(-3.04);
+        GotoThet(-PI/2.f);
         R_SEUIL_SHARP = 0.35;
         GotoDist(5.5);
         R_SEUIL_SHARP = 1;
         GotoDist(4.5);
         GotoArr(3);
         R_SEUIL_SHARP = 0.35;
-        GotoThet(3.04);
+        GotoThet(PI/2.f);
         GotoDist(3.5);
-        GotoThet(-3.04);
+        GotoThet(-PI/2.f);
         R_SEUIL_SHARP = 1; 
         GotoDist(4.6);
     }
-    else if (SCouleur == VIOLET) {  
+    else// if (SCouleur == VIOLET)
+    {  
         end.attach(&endFonc, 90);
-        GotoDist(9.0);
-        GotoArr(9.2);
+        GotoDist(9.0,2000,2000,2000);
+        GotoArr(5);
         R_SEUIL_SHARP = 0.25;
-        GotoDist(2.5);
-        GotoThet(3.04);
+        GotoDist(1.25);
+        GotoThet(PI/2.f);
         R_SEUIL_SHARP = 0.35;
-        GotoDist(5.5);
+        GotoDist(2.75);
         R_SEUIL_SHARP = 1;
-        GotoDist(4.5);
-        GotoArr(4);
+        GotoDist(2.25);
+        GotoArr(2);
         R_SEUIL_SHARP = 0.35;
-        GotoThet(-3.04);
-        GotoDist(3.5);
-        GotoThet(3.04);
+        GotoThet(-PI/2.f);
+        GotoDist(1.75);
+        GotoThet(PI/2.f);
         R_SEUIL_SHARP = 1; 
-        GotoDist(4.6);
+        GotoDist(2.3);
     }
-    else {
-        GotoDistPos(100);
+   /* else {
+        GotoDistPos(200);
         GotoThet(PI/2.f);
-        GotoDistPos(200);
+        GotoDistPos(400);
         
         GotoThet(PI/2.f);
         GotoThet(PI/2.f);
         
-        GotoDistPos(200);
+        GotoDistPos(400);
         
         GotoThet(-PI/2.f);
         
-        GotoDistPos(100);
+        GotoDistPos(200);
         GotoThet(PI/2.f);
         GotoThet(PI/2.f);
-    }
+    }*/
 
     while(1);
 }