Diff: Odometry/Odometry.cpp

Revision:

14:d5e21f71c2a9

Parent:

10:ae3178aa94e9

Child:

30:58bfac39e701

--- a/Odometry/Odometry.cpp	Fri Dec 04 11:18:13 2015 +0000
+++ b/Odometry/Odometry.cpp	Tue Jan 05 14:51:10 2016 +0000
@@ -26,6 +26,10 @@
     this->y = y;
     this->theta = theta;
 }
+void Odometry::getEnc()
+{
+    pc.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
+}
 
 void Odometry::setX(double x)
 {
@@ -48,11 +52,11 @@
     m_pulses_right = roboclaw.ReadEncM1(ADR);
     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)*C / 2.0f;
+    double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
 
-    double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
-    double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right) / m_v;
-
-    double R = deltaS/deltaTheta;
+    /*double R = deltaS/deltaTheta;
 
     double xO = x - R*sin(theta);
     double yO = y + R*cos(theta);
@@ -66,26 +70,35 @@
     else {
         x = xO + R*sin(theta);
         y = yO - R*cos(theta);
-    }
+    }*/
 
-    /*double dx = deltaS*cos(theta);
+    double dx = deltaS*cos(theta);
     double dy = deltaS*sin(theta);
     x += dx;
     y += dy;
-    theta += deltaTheta;*/
+    theta += deltaTheta;
 
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
 }
 
+void Odometry::GotoXY(double x_goal, double y_goal)
+{
+    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);
+    GotoThet(theta_);
+    GotoDist(dist_);
+}
+
 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
 {
     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);
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
+    pc.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
+    GotoThet(theta_goal);
 }
 
 void Odometry::GotoThet(double theta_)
@@ -104,18 +117,20 @@
     double erreur_theta = theta_ - getTheta();
 
     while(erreur_theta >= PI) erreur_theta -= 2*PI;
-    while(erreur_theta <= -PI) erreur_theta += 2*PI;
-
-    if(erreur_theta <= 0) {
-        distance_ticks_left = (int32_t) -(erreur_theta*m_v/2)/m_distPerTick_left + pos_initiale_left;
-        distance_ticks_right = (int32_t) (erreur_theta*m_v/2)/m_distPerTick_right + pos_initiale_right;
+    while(erreur_theta < -PI) erreur_theta += 2*PI;
+    
+    pc.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;
+        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     } else {
-        distance_ticks_left = (int32_t) (erreur_theta*m_v/2)/m_distPerTick_left + pos_initiale_left;
-        distance_ticks_right = (int32_t) -(erreur_theta*m_v/2)/m_distPerTick_right + pos_initiale_right;
+        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
+        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
     }
 
-    //pc.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
-    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("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);
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
@@ -123,7 +138,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("%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_);
+    setTheta(theta_);
     led = 1;
     //arrived = true;
     //pc.printf("arrivey %d\n\r",pos_prog);
@@ -144,9 +159,8 @@
     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);
-
+    
     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);
-
     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);