Diff: Odometry/Odometry.cpp

Revision:

38:da3a2c781672

Parent:

36:2d7357a385bc

Child:

39:309f38d1e49c

Child:

40:ca4dd3faffa8

--- a/Odometry/Odometry.cpp	Sun Jan 31 16:11:32 2016 +0000
+++ b/Odometry/Odometry.cpp	Sat Feb 06 10:11:28 2016 +0000
@@ -28,7 +28,7 @@
 }
 void Odometry::getEnc()
 {
-    pc.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
+    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
 }
 
 void Odometry::setX(double x)
@@ -91,7 +91,7 @@
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    pc.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_);
 }
@@ -100,7 +100,7 @@
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    pc.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);
@@ -110,7 +110,7 @@
 {
     led = 0;
     //pos_prog++;
-    //pc.printf("Theta : %3.2f\n\r", theta_*180/PI);
+    //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
 
     int32_t distance_ticks_left;
@@ -124,7 +124,7 @@
     while(erreur_theta >= PI) erreur_theta -= 2*PI;
     while(erreur_theta < -PI) erreur_theta += 2*PI;
     
-    pc.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
+    logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
     
     if(erreur_theta < 0) {
         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
@@ -134,27 +134,27 @@
         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     }
 
-    //pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
-    //pc.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
+    //logger.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
+    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
+    //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);
 
-    //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+    //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)); //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);
+    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);
     wait(0.4);
     setTheta(theta_);
     led = 1;
     //arrived = true;
-    //pc.printf("arrivey %d\n\r",pos_prog);
+    //logger.printf("arrivey %d\n\r",pos_prog);
 }
 
 void Odometry::GotoDist(double distance)
 {
     led = 0;
     //pos_prog++;
-    //pc.printf("Dist : %3.2f\n\r", distance);
+    //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
 
     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
@@ -164,13 +164,13 @@
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
 
-    //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+    //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)); //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);
+    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;
-    //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);
+    //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);
 }
 
 void Odometry::TestEntraxe(int i) {