Diff: Odometry/Odometry.cpp

Revision:

76:a862cb10559c

Parent:

73:538d6627ae2f

--- a/Odometry/Odometry.cpp	Thu May 05 04:36:59 2016 +0000
+++ b/Odometry/Odometry.cpp	Thu May 05 06:34:13 2016 +0000
@@ -107,7 +107,6 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    logger.printf("Odometry::GotoThet...");
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -141,40 +140,35 @@
 
     //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 ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left); 
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left))
+        logger.printf ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left); 
     //logger.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
     
     wait(0.4);
     setTheta(theta_);
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
-    logger.printf("End\n\r");
 }
 
 void Odometry::GotoDist(double distance)
 {
-    logger.printf("Odometry::GotoDist...");
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
-    bool avant = distance >= 0 ? true: false;
 
     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.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
     //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);
-    
     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");
 }
 
 void Odometry::TestEntraxe(int i) {
@@ -194,8 +188,8 @@
     
     roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
     
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
-       // logger.printf ("[Dist] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left))
+        logger.printf ("[Dist] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left);
         
     wait(0.4);
 }