Diff: deplacement.cpp

Revision:

96:1e91cac784fe

Parent:

94:86b9bd6d5c28

Child:

97:42167cfeb8d7

--- a/deplacement.cpp	Fri May 06 18:25:24 2016 +0000
+++ b/deplacement.cpp	Fri May 06 21:21:49 2016 +0000
@@ -69,10 +69,8 @@
 void GotoThet(double theta_) {
     roboclaw.ResetEnc();
     wait_ms(10);
-    float diameter_left = 61.7;
-    float diameter_right = 61.8;
-    int distance_ticks_left = -theta_*ENTRAXE/(2*(diameter_left*3.14159/4096));
-    int distance_ticks_right = theta_*ENTRAXE/(2*(diameter_right*3.14159/4096));
+    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);
@@ -84,21 +82,16 @@
     roboclaw.ForwardM2(0);
 }
 
-void Odometry::GotoDistPos(double distance)
+void GotoDistPos(double distance)
 {
-    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
-
-    int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
-    int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
-
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_left, 1);
+    roboclaw.ResetEnc();
+    wait_ms(1);
+    int32_t distance_ticks_left = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
+    int32_t distance_ticks_right = distance/((DIAMETRE_ROUE_GAUCHE*3.14156)/4096);
 
-    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
-    
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
+    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(0.4);
-    //logger.printf("arrivey %d\n\r",pos_prog);
-    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
-    logger.printf("End\n\r");
+    wait(0.1);
 }
\ No newline at end of file