ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

Odometry/Odometry.cpp@4:3e6e78d6d3d9, 2015-11-24 (annotated)

Committer:
sype
Date:
Tue Nov 24 21:56:23 2015 +0000
Revision:
4:3e6e78d6d3d9
Parent:
3:62e9d715de65
Child:
10:ae3178aa94e9
Mise en forme

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 0:ad9600df4a70 1 #include "Odometry.h"
sype 0:ad9600df4a70 2
sype 0:ad9600df4a70 3 // M1 = Moteur droit, M2 = Moteur gauche
sype 0:ad9600df4a70 4
sype 4:3e6e78d6d3d9 5 Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc){
sype 0:ad9600df4a70 6 m_v = v;
sype 0:ad9600df4a70 7 m_distPerTick_left = diameter_left*PI/37400;
sype 0:ad9600df4a70 8 m_distPerTick_right = diameter_right*PI/37400;
sype 4:3e6e78d6d3d9 9
sype 3:62e9d715de65 10 roboclaw.ForwardM1(ADR, 0);
sype 3:62e9d715de65 11 roboclaw.ForwardM2(ADR, 0);
sype 4:3e6e78d6d3d9 12
sype 4:3e6e78d6d3d9 13 // Erreur autorisée sur le déplacement en angle
sype 3:62e9d715de65 14 erreur_ang = 0.01;
sype 4:3e6e78d6d3d9 15
sype 0:ad9600df4a70 16 m_pulses_right = 0;
sype 0:ad9600df4a70 17 m_pulses_left = 0;
sype 2:abdf8c6823a1 18 wait_ms(100);
sype 0:ad9600df4a70 19 }
sype 0:ad9600df4a70 20
sype 4:3e6e78d6d3d9 21 void Odometry::setPos(double x, double y, double theta){
sype 0:ad9600df4a70 22 this->x = x;
sype 0:ad9600df4a70 23 this->y = y;
sype 0:ad9600df4a70 24 this->theta = theta;
sype 0:ad9600df4a70 25 }
sype 0:ad9600df4a70 26
sype 4:3e6e78d6d3d9 27 void Odometry::setX(double x){
sype 0:ad9600df4a70 28 this->x = x;
sype 0:ad9600df4a70 29 }
sype 0:ad9600df4a70 30
sype 4:3e6e78d6d3d9 31 void Odometry::setY(double y){
sype 0:ad9600df4a70 32 this->y = y;
sype 0:ad9600df4a70 33 }
sype 0:ad9600df4a70 34
sype 4:3e6e78d6d3d9 35 void Odometry::setTheta(double theta){
sype 0:ad9600df4a70 36 this->theta = theta;
sype 0:ad9600df4a70 37 }
sype 0:ad9600df4a70 38
sype 4:3e6e78d6d3d9 39 void Odometry::update_odo(void){
sype 3:62e9d715de65 40 int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
sype 0:ad9600df4a70 41 m_pulses_right = roboclaw.ReadEncM1(ADR);
sype 3:62e9d715de65 42 int32_t delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
sype 0:ad9600df4a70 43 m_pulses_left = roboclaw.ReadEncM2(ADR);
sype 4:3e6e78d6d3d9 44
sype 0:ad9600df4a70 45 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
sype 2:abdf8c6823a1 46 double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
sype 4:3e6e78d6d3d9 47
sype 2:abdf8c6823a1 48 double radius = deltaS/deltaTheta;
sype 2:abdf8c6823a1 49 double xO = x - radius*sin(theta);
sype 2:abdf8c6823a1 50 double yO = y + radius*cos(theta);
sype 4:3e6e78d6d3d9 51
sype 0:ad9600df4a70 52 theta += deltaTheta;
sype 4:3e6e78d6d3d9 53
sype 2:abdf8c6823a1 54 x = xO + radius*sin(theta);
sype 2:abdf8c6823a1 55 y = yO - radius*cos(theta);
sype 4:3e6e78d6d3d9 56
sype 0:ad9600df4a70 57 while(theta > PI) theta -= 2*PI;
sype 0:ad9600df4a70 58 while(theta <= -PI) theta += 2*PI;
sype 0:ad9600df4a70 59 }
sype 0:ad9600df4a70 60
sype 4:3e6e78d6d3d9 61 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal){
sype 0:ad9600df4a70 62 double theta_ = atan2(y_goal-y, x_goal-x);
sype 3:62e9d715de65 63 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 2:abdf8c6823a1 64 GotoThet(theta_);
sype 3:62e9d715de65 65 GotoDist(dist_);
sype 0:ad9600df4a70 66 }
sype 2:abdf8c6823a1 67
sype 4:3e6e78d6d3d9 68 void Odometry::GotoThet(double theta_) {
sype 2:abdf8c6823a1 69 double distance_ticks_left;
sype 2:abdf8c6823a1 70 double distance_ticks_right;
sype 4:3e6e78d6d3d9 71
sype 4:3e6e78d6d3d9 72 // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
sype 3:62e9d715de65 73 double erreur_theta = theta_ - getTheta();
sype 3:62e9d715de65 74 bool arrived = false;
sype 4:3e6e78d6d3d9 75
sype 3:62e9d715de65 76 while(erreur_theta >= PI) erreur_theta -= 2*PI;
sype 2:abdf8c6823a1 77 while(erreur_theta <= -PI) erreur_theta += 2*PI;
sype 4:3e6e78d6d3d9 78
sype 4:3e6e78d6d3d9 79 if(erreur_theta <= 0) {
sype 2:abdf8c6823a1 80 distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
sype 2:abdf8c6823a1 81 distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
sype 4:3e6e78d6d3d9 82 } else {
sype 2:abdf8c6823a1 83 distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
sype 2:abdf8c6823a1 84 distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
sype 2:abdf8c6823a1 85 }
sype 3:62e9d715de65 86 pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
sype 3:62e9d715de65 87 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 100000, (int32_t)distance_ticks_right, 150000, 150000, 100000, (int32_t)distance_ticks_left, 1);
sype 4:3e6e78d6d3d9 88 // Il faut ici faire un espèce de bouclage pour vérifier qu'il est bien arrivé, j'avais pensé à :
sype 4:3e6e78d6d3d9 89 /*
sype 4:3e6e78d6d3d9 90 while(!arrived){
sype 4:3e6e78d6d3d9 91
sype 4:3e6e78d6d3d9 92 }
sype 4:3e6e78d6d3d9 93 */
sype 2:abdf8c6823a1 94 }
sype 2:abdf8c6823a1 95
sype 4:3e6e78d6d3d9 96 void Odometry::GotoDist(double distance) {
sype 3:62e9d715de65 97 double temp1 = roboclaw.ReadEncM1(ADR), temp2 = roboclaw.ReadEncM2(ADR);
sype 3:62e9d715de65 98 double distance_ticks_left = distance/m_distPerTick_left - temp2;
sype 3:62e9d715de65 99 double distance_ticks_right = distance/m_distPerTick_right - temp1;
sype 3:62e9d715de65 100 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (int32_t)distance_ticks_right, 150000, 200000, 150000, (int32_t)distance_ticks_left, 1);
sype 2:abdf8c6823a1 101 }
sype 2:abdf8c6823a1 102
sype 4:3e6e78d6d3d9 103 bool Odometry::isArrivedRot(double theta_) {
sype 3:62e9d715de65 104 if(abs_d(getTheta()) <= abs_d(theta_+erreur_ang)) return true;
sype 3:62e9d715de65 105 else if(abs_d(getTheta()) >= abs_d(theta_-erreur_ang)) return true;
sype 2:abdf8c6823a1 106 else return false;
sype 2:abdf8c6823a1 107 }