Diff: Odometry/Odometry.cpp

Revision:

46:5658af4e5149

Parent:

39:309f38d1e49c

Child:

49:5e2f7323f280

--- a/Odometry/Odometry.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ b/Odometry/Odometry.cpp	Mon Apr 25 12:38:58 2016 +0000
@@ -8,9 +8,9 @@
     m_distPerTick_left = diameter_left*PI/quadrature;
     m_distPerTick_right = diameter_right*PI/quadrature;
 
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    roboclaw.ResetEnc(ADR);
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    roboclaw.ResetEnc();
     // Erreur autorisée sur le déplacement en angle
     erreur_ang = 0.01;
 
@@ -28,7 +28,7 @@
 }
 void Odometry::getEnc()
 {
-    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
+    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(), roboclaw.ReadEncM2());
 }
 
 void Odometry::setX(double x)
@@ -48,8 +48,8 @@
 
 void Odometry::update_odo(void)
 {
-    int32_t roboclawENCM1 = roboclaw.ReadEncM1(ADR); 
-    int32_t roboclawENCM2 = roboclaw.ReadEncM2(ADR); 
+    int32_t roboclawENCM1 = roboclaw.ReadEncM1(); 
+    int32_t roboclawENCM2 = roboclaw.ReadEncM2();
     int32_t delta_right = roboclawENCM1 - m_pulses_right;
     m_pulses_right = roboclawENCM1;
     int32_t delta_left = roboclawENCM2 - m_pulses_left;
@@ -84,7 +84,6 @@
     theta += deltaTheta;
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
-    
 }
 
 void Odometry::GotoXY(double x_goal, double y_goal)
@@ -108,7 +107,6 @@
 
 void Odometry::GotoThet(double theta_)
 {
-    led = 0;
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
@@ -138,21 +136,19 @@
     //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);
+    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, 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("%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;
     //logger.printf("arrivey %d\n\r",pos_prog);
 }
 
 void Odometry::GotoDist(double distance)
 {
-    led = 0;
     //pos_prog++;
     //logger.printf("Dist : %3.2f\n\r", distance);
     //arrived = false;
@@ -162,13 +158,12 @@
     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(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, 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);
-    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);
 }
@@ -188,7 +183,7 @@
         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     }
     
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    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)); 
     wait(0.4);
@@ -200,11 +195,10 @@
     int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
     int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
 
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, 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);
-    led = 1;
 }
\ No newline at end of file