Diff: Odometry/Odometry.cpp

Revision:

4:3e6e78d6d3d9

Parent:

3:62e9d715de65

Child:

10:ae3178aa94e9

--- a/Odometry/Odometry.cpp	Tue Nov 24 21:42:10 2015 +0000
+++ b/Odometry/Odometry.cpp	Tue Nov 24 21:56:23 2015 +0000
@@ -2,108 +2,105 @@
 
 // M1 = Moteur droit, M2 = Moteur gauche
 
-Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc)
-{
+Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc){
     m_v = v;
     m_distPerTick_left = diameter_left*PI/37400;
     m_distPerTick_right = diameter_right*PI/37400;
-    
+
     roboclaw.ForwardM1(ADR, 0);
     roboclaw.ForwardM2(ADR, 0);
-    
+
+    // Erreur autorisée sur le déplacement en angle
     erreur_ang = 0.01;
+
     m_pulses_right = 0;
     m_pulses_left = 0;
     wait_ms(100);
 }
 
-void Odometry::setPos(double x, double y, double theta)
-{
+void Odometry::setPos(double x, double y, double theta){
     this->x = x;
     this->y = y;
     this->theta = theta;
 }
 
-void Odometry::setX(double x)
-{
+void Odometry::setX(double x){
     this->x = x;
 }
 
-void Odometry::setY(double y)
-{
+void Odometry::setY(double y){
     this->y = y;
 }
 
-void Odometry::setTheta(double theta)
-{
+void Odometry::setTheta(double theta){
     this->theta = theta;
 }
 
-void Odometry::update_odo(void)
-{
+void Odometry::update_odo(void){
     int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
     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) / 2.0f;
     double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
-    
+
     double radius = deltaS/deltaTheta;
     double xO = x - radius*sin(theta);
     double yO = y + radius*cos(theta);
-    
+
     theta += deltaTheta;
-    
+
     x = xO + radius*sin(theta);
     y = yO - radius*cos(theta);
-    
+
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
 }
 
-void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
-{
+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));
     GotoThet(theta_);
     GotoDist(dist_);
 }
 
-void Odometry::GotoThet(double theta_)
-{
+void Odometry::GotoThet(double theta_) {
     double distance_ticks_left;
     double distance_ticks_right;
+
+    // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
     double erreur_theta = theta_ - getTheta();
     bool arrived = false;
-    
+
     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;
-    }
-    else
-    {
+    } else {
         distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
         distance_ticks_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);
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 100000, (int32_t)distance_ticks_right, 150000, 150000, 100000, (int32_t)distance_ticks_left, 1);
+    // Il faut ici faire un espèce de bouclage pour vérifier qu'il est bien arrivé, j'avais pensé à :
+    /*
+    while(!arrived){
+        
+    }
+    */
 }
 
-void Odometry::GotoDist(double distance)
-{
+void Odometry::GotoDist(double distance) {
     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_)
-{
+bool Odometry::isArrivedRot(double theta_) {
     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;