Diff: Odometry/Odometry.cpp

Revision:

46:8eae88c45a78

Parent:

39:309f38d1e49c

--- a/Odometry/Odometry.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Odometry/Odometry.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -1,5 +1,6 @@
 #include "Odometry.h"
 
+// GotoThet : settheta enleve.
 // M1 = Moteur droit, M2 = Moteur gauche
 
 Odometry::Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
@@ -108,7 +109,7 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    led = 0;
+    S::led = 0;
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -142,17 +143,17 @@
 
     //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);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)) logger.printf("[Theta] %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_);
-    led = 1;
+    //setTheta(theta_);
+    S::led = 1;
     //arrived = true;
-    //logger.printf("arrivey %d\n\r",pos_prog);
+    logger.printf("[End GotoThet] X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
 
 void Odometry::GotoDist(double distance)
 {
-    led = 0;
+    S::led = 0;
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
@@ -166,11 +167,11 @@
 
     //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);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); logger.printf("[Dist] 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;
+    S::led = 1;
     //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 GotoDist] X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
 
 void Odometry::TestEntraxe(int i) {
@@ -204,7 +205,7 @@
 
     //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);
+    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);
-    led = 1;
+    S::led = 1;
 }
\ No newline at end of file