Diff: Odometry/Odometry.cpp

Revision:

3:62e9d715de65

Parent:

2:abdf8c6823a1

Child:

4:3e6e78d6d3d9

--- a/Odometry/Odometry.cpp	Tue Nov 24 15:02:01 2015 +0000
+++ b/Odometry/Odometry.cpp	Tue Nov 24 21:42:10 2015 +0000
@@ -8,7 +8,10 @@
     m_distPerTick_left = diameter_left*PI/37400;
     m_distPerTick_right = diameter_right*PI/37400;
     
-    erreur_ang = 0.1;
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
+    erreur_ang = 0.01;
     m_pulses_right = 0;
     m_pulses_left = 0;
     wait_ms(100);
@@ -38,9 +41,9 @@
 
 void Odometry::update_odo(void)
 {
-    long delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
+    int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
     m_pulses_right = roboclaw.ReadEncM1(ADR);
-    long delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
+    int32_t delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
     m_pulses_left = roboclaw.ReadEncM2(ADR);
     
     double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
@@ -62,20 +65,22 @@
 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
-    float distance = sqrt(carre(x_goal-x)+carre(y_goal-y));
+    double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
     GotoThet(theta_);
+    GotoDist(dist_);
 }
 
 void Odometry::GotoThet(double theta_)
 {
     double distance_ticks_left;
     double distance_ticks_right;
-    double erreur_theta = theta_-getTheta();
+    double erreur_theta = theta_ - getTheta();
+    bool arrived = false;
     
-    while(erreur_theta > PI) erreur_theta -= 2*PI;
+    while(erreur_theta >= PI) erreur_theta -= 2*PI;
     while(erreur_theta <= -PI) erreur_theta += 2*PI;
     
-    if(erreur_theta >= 0)
+    if(erreur_theta <= 0)
     {
         distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
         distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
@@ -85,22 +90,21 @@
         distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
         distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
     }
-    
-    pc.printf("T_%3.2f\t T%3.2f\t ET%3.2f\n\r",theta_*180/PI, getTheta()*180/PI, erreur_theta*180/PI);
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 150000, (long)distance_ticks_right, 150000, 150000, 150000, (long)distance_ticks_left, 1);
-    while(isArrivedRot(erreur_theta))pc.printf("Theta : %3.2f\n\r",getTheta()*180/PI);;
-    pc.printf("Arrived");
+    pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
+    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 100000, (int32_t)distance_ticks_right, 150000, 150000, 100000, (int32_t)distance_ticks_left, 1);
 }
 
-void Odometry::GotoB(double distance)
+void Odometry::GotoDist(double distance)
 {
-    double distance_ticks_left = distance/m_distPerTick_left;
-    double distance_ticks_right =  distance/m_distPerTick_right;
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (long)distance_ticks_right, 150000, 200000, 150000, (long)distance_ticks_left, 1);
+    double temp1 = roboclaw.ReadEncM1(ADR), temp2 = roboclaw.ReadEncM2(ADR);
+    double distance_ticks_left = distance/m_distPerTick_left - temp2;
+    double distance_ticks_right =  distance/m_distPerTick_right - temp1;
+    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (int32_t)distance_ticks_right, 150000, 200000, 150000, (int32_t)distance_ticks_left, 1);
 }
 
 bool Odometry::isArrivedRot(double theta_)
 {
-    if((abs_d(getTheta())<=abs_d(theta_)+erreur_ang)|(abs_d(getTheta())>=abs_d(theta_)-erreur_ang)) return true;
+    if(abs_d(getTheta()) <= abs_d(theta_+erreur_ang)) return true;
+    else if(abs_d(getTheta()) >= abs_d(theta_-erreur_ang)) return true;
     else return false;
 }