Diff: Odometry/Odometry.cpp

Revision:

31:8bcc3a0bfa8a

Parent:

30:58bfac39e701

Child:

32:068bd2b2e1f3

--- a/Odometry/Odometry.cpp	Fri Jan 22 15:46:43 2016 +0000
+++ b/Odometry/Odometry.cpp	Tue Jan 26 16:39:43 2016 +0000
@@ -138,7 +138,8 @@
     //pc.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)); //pc.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);
-    //setTheta(theta_);
+    wait(0.4);
+    setTheta(theta_);
     led = 1;
     //arrived = true;
     //pc.printf("arrivey %d\n\r",pos_prog);
@@ -161,6 +162,7 @@
     //pc.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)); //pc.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);
     led = 1;
     //pc.printf("arrivey %d\n\r",pos_prog);
     //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);