Revision 97:42167cfeb8d7, committed 2016-05-06

Comitter:

sype

Date:

Fri May 06 22:15:43 2016 +0000

Parent:

96:1e91cac784fe

Child:

98:2426d699362b

Commit message:

update gotoDistPos

Changed in this revision

--- a/ControlleurPince/ControlleurPince.h	Fri May 06 21:21:49 2016 +0000
+++ b/ControlleurPince/ControlleurPince.h	Fri May 06 22:15:43 2016 +0000
@@ -3,9 +3,12 @@
 
 //#include "defines.h"
 #include "Stepper.h"
-#include "../AX12/AX12.h"
+#include "AX12.h"
 #include "mbed.h"
-#include "defines_Pince.h"
+
+#define HAUT 1
+#define BAS 0
+#define DELAY 0.007
 
 class ControlleurPince
 {

--- a/ControlleurPince/defines_Pince.h	Fri May 06 21:21:49 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,3 +0,0 @@
-#define HAUT 1
-#define BAS 0
-#define DELAY 0.007
\ No newline at end of file

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

--- a/entete.h	Fri May 06 21:21:49 2016 +0000
+++ b/entete.h	Fri May 06 22:15:43 2016 +0000
@@ -4,6 +4,8 @@
 #include "RoboClaw/RoboClaw.h"
 #include "mbed.h"
 
+#define PI 3.14159f
+
 /* Classes du projet : 
 AX12
 Roboclaw
@@ -39,6 +41,7 @@
 void GotoDist (float timer);
 void GotoArr(float timer);
 void GotoThet(double theta_);
+void GotoDistPos(double distance);
 
 // Fonctions test.cpp
 
@@ -54,9 +57,9 @@
 #define vitesse_angle 3200
 #define deccel_angle 3200
 
-#define accel_dista 2000
-#define vitesse_dista 2000
-#define deccel_dista 2000
+#define accel_dista 4000
+#define vitesse_dista 4000
+#define deccel_dista 4000
 
 #define waiting_time 1
 #define GAUCHE 1

--- a/main.cpp	Fri May 06 21:21:49 2016 +0000
+++ b/main.cpp	Fri May 06 22:15:43 2016 +0000
@@ -86,9 +86,21 @@
         R_SEUIL_SHARP = 1; 
         GotoDist(4.6);
     }
-    else if (SCouleur == NOIR) {
-        GotoThet(-3.14159/2);
-        GotoDist(1);
+    else {
+        GotoDistPos(100);
+        GotoThet(PI/2.f);
+        GotoDistPos(200);
+        
+        GotoThet(PI/2.f);
+        GotoThet(PI/2.f);
+        
+        GotoDistPos(200);
+        
+        GotoThet(-PI/2.f);
+        
+        GotoDistPos(100);
+        GotoThet(PI/2.f);
+        GotoThet(PI/2.f);
     }
 
     while(1);
@@ -156,5 +168,6 @@
     }
    
     wait(1);
+    LEDV = 1;
     depart();
 }