Odometry.cpp

00001 #include "Odometry.h"
00002 
00003 // GotoThet : settheta enleve.
00004 // M1 = Moteur droit, M2 = Moteur gauche
00005 
00006 Odometry::Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
00007 {
00008     m_v = v;
00009     m_distPerTick_left = diameter_left*PI/quadrature;
00010     m_distPerTick_right = diameter_right*PI/quadrature;
00011 
00012     roboclaw.ForwardM1(ADR, 0);
00013     roboclaw.ForwardM2(ADR, 0);
00014     roboclaw.ResetEnc(ADR);
00015     // Erreur autorisée sur le déplacement en angle
00016     erreur_ang = 0.01;
00017 
00018     m_pulses_right = 0;
00019     m_pulses_left = 0;
00020     pos_prog = 0;
00021     wait_ms(100);
00022 }
00023 
00024 void Odometry::setPos(double x, double y, double theta)
00025 {
00026     this->x = x;
00027     this->y = y;
00028     this->theta = theta;
00029 }
00030 void Odometry::getEnc()
00031 {
00032     logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
00033 }
00034 
00035 void Odometry::setX(double x)
00036 {
00037     this->x = x;
00038 }
00039 
00040 void Odometry::setY(double y)
00041 {
00042     this->y = y;
00043 }
00044 
00045 void Odometry::setTheta(double theta)
00046 {
00047     this->theta = theta;
00048 }
00049 
00050 void Odometry::update_odo(void)
00051 {
00052     int32_t roboclawENCM1 = roboclaw.ReadEncM1(ADR); 
00053     int32_t roboclawENCM2 = roboclaw.ReadEncM2(ADR); 
00054     int32_t delta_right = roboclawENCM1 - m_pulses_right;
00055     m_pulses_right = roboclawENCM1;
00056     int32_t delta_left = roboclawENCM2 - m_pulses_left;
00057     m_pulses_left = roboclawENCM2;
00058     
00059     double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right)*C / 2.0f;
00060     double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
00061 
00062     /*double R = deltaS/deltaTheta;
00063 
00064     double xO = x - R*sin(theta);
00065     double yO = y + R*cos(theta);
00066 
00067     theta += deltaTheta;
00068 
00069     if(deltaTheta == 0) {
00070         x = x + deltaS*cos(theta);
00071         y = y + deltaS*sin(theta);
00072     }
00073     else {
00074         x = xO + R*sin(theta);
00075         y = yO - R*cos(theta);
00076     }*/
00077     
00078     
00079     
00080     double dx = deltaS*cos(theta+deltaTheta/2);
00081     double dy = deltaS*sin(theta+deltaTheta/2);
00082     x += dx;
00083     y += dy;
00084     
00085     theta += deltaTheta;
00086     while(theta > PI) theta -= 2*PI;
00087     while(theta <= -PI) theta += 2*PI;
00088     
00089 }
00090 
00091 void Odometry::GotoXY(double x_goal, double y_goal)
00092 {
00093     double theta_ = atan2(y_goal-y, x_goal-x);
00094     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
00095     logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
00096     GotoThet(theta_);
00097     GotoDist(dist_);
00098 }
00099 
00100 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
00101 {
00102     double theta_ = atan2(y_goal-y, x_goal-x);
00103     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
00104     logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
00105     GotoThet(theta_);
00106     GotoDist(dist_);
00107     GotoThet(theta_goal);
00108 }
00109 
00110 void Odometry::GotoThet(double theta_)
00111 {
00112     S::led = 0;
00113     //pos_prog++;
00114     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
00115     //arrived = false;
00116 
00117     int32_t distance_ticks_left;
00118     int32_t distance_ticks_right;
00119 
00120     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
00121 
00122     // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
00123     double erreur_theta = theta_ - getTheta();
00124 
00125     while(erreur_theta >= PI) erreur_theta -= 2*PI;
00126     while(erreur_theta < -PI) erreur_theta += 2*PI;
00127     
00128     logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
00129     
00130     if(erreur_theta < 0) {
00131         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
00132         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
00133     } else {
00134         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
00135         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
00136     }
00137 
00138     //logger.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
00139     //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
00140     //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
00141 
00142     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
00143 
00144     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
00145 
00146     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)) logger.printf("[Theta] %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);
00147     wait(0.4);
00148     //setTheta(theta_);
00149     S::led = 1;
00150     //arrived = true;
00151     logger.printf("[End GotoThet] X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
00152 }
00153 
00154 void Odometry::GotoDist(double distance)
00155 {
00156     S::led = 0;
00157     //pos_prog++;
00158     //logger.printf("Dist : %3.2f\n\r", distance);
00159     //arrived = false;
00160 
00161     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
00162 
00163     int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
00164     int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
00165 
00166     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
00167 
00168     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
00169     
00170     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); logger.printf("[Dist] 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);
00171     wait(0.4);
00172     S::led = 1;
00173     //logger.printf("arrivey %d\n\r",pos_prog);
00174     logger.printf("[End GotoDist] X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
00175 }
00176 
00177 void Odometry::TestEntraxe(int i) {
00178     int32_t distance_ticks_left;
00179     int32_t distance_ticks_right;
00180 
00181     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
00182 
00183     double erreur_theta = 2*PI*i - getTheta();
00184     if(erreur_theta < 0) {
00185         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
00186         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
00187     } else {
00188         distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
00189         distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
00190     }
00191     
00192     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
00193     
00194     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); 
00195     wait(0.4);
00196 }
00197 
00198 void Odometry::Forward(float i) {    
00199     int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
00200 
00201     int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
00202     int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
00203 
00204     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
00205 
00206     //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
00207     
00208     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);
00209     wait(0.4);
00210     S::led = 1;
00211 }