Diff: Odometry/Odometry.cpp

Revision:

10:ae3178aa94e9

Parent:

4:3e6e78d6d3d9

Child:

14:d5e21f71c2a9

diff -r e39b218ab20d -r ae3178aa94e9 Odometry/Odometry.cpp
--- a/Odometry/Odometry.cpp	Tue Nov 24 23:05:12 2015 +0000
+++ b/Odometry/Odometry.cpp	Fri Dec 04 11:18:13 2015 +0000
@@ -2,106 +2,152 @@
 
 // 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, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
+{
     m_v = v;
-    m_distPerTick_left = diameter_left*PI/37400;
-    m_distPerTick_right = diameter_right*PI/37400;
+    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);
     // Erreur autorisée sur le déplacement en angle
     erreur_ang = 0.01;
 
     m_pulses_right = 0;
     m_pulses_left = 0;
+    pos_prog = 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 deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right) / m_v;
 
-    double radius = deltaS/deltaTheta;
-    double xO = x - radius*sin(theta);
-    double yO = y + radius*cos(theta);
+    double R = deltaS/deltaTheta;
+
+    double xO = x - R*sin(theta);
+    double yO = y + R*cos(theta);
 
     theta += deltaTheta;
 
-    x = xO + radius*sin(theta);
-    y = yO - radius*cos(theta);
+    if(deltaTheta == 0) {
+        x = x + deltaS*cos(theta);
+        y = y + deltaS*sin(theta);
+    }
+    else {
+        x = xO + R*sin(theta);
+        y = yO - R*cos(theta);
+    }
+
+    /*double dx = deltaS*cos(theta);
+    double dy = deltaS*sin(theta);
+    x += dx;
+    y += dy;
+    theta += deltaTheta;*/
 
     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));
+    //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);
     GotoThet(theta_);
     GotoDist(dist_);
 }
 
-void Odometry::GotoThet(double theta_) {
-    double distance_ticks_left;
-    double distance_ticks_right;
+void Odometry::GotoThet(double theta_)
+{
+    led = 0;
+    //pos_prog++;
+    //pc.printf("Theta : %3.2f\n\r", theta_*180/PI);
+    //arrived = false;
+
+    int32_t distance_ticks_left;
+    int32_t distance_ticks_right;
+
+    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
 
     // 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) {
-        distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
+        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;
     } else {
-        distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
+        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;
     }
+
+    //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);
-    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){
-        
-    }
-    */
+    //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);
+
+    //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_);
+    led = 1;
+    //arrived = true;
+    //pc.printf("arrivey %d\n\r",pos_prog);
 }
 
-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);
+void Odometry::GotoDist(double distance)
+{
+    led = 0;
+    //pos_prog++;
+    //pc.printf("Dist : %3.2f\n\r", distance);
+    //arrived = false;
+
+    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
+
+    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);
+
+    //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);
 }
-
-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;
-}