Diff: Odometry/Odometry.cpp

Revision:

40:ca4dd3faffa8

Parent:

38:da3a2c781672

--- a/Odometry/Odometry.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ b/Odometry/Odometry.cpp	Wed Apr 13 11:27:01 2016 +0000
@@ -90,9 +90,11 @@
 void Odometry::GotoXY(double x_goal, double y_goal)
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
+    logger.printf("odo::GotoXY %f\n\r", theta_);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
     logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
+    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoDist(dist_);
 }
 
@@ -100,7 +102,7 @@
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
+    //logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
     GotoThet(theta_goal);
@@ -139,7 +141,9 @@
     //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
-
+    
+    logger.printf("On a enclenché le mouvement");
+    
     //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("%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);