Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

Odometry/Odometry.cpp@3:62e9d715de65, 2015-11-24 (annotated)

Committer:
sype
Date:
Tue Nov 24 21:42:10 2015 +0000
Revision:
3:62e9d715de65
Parent:
2:abdf8c6823a1
Child:
4:3e6e78d6d3d9
GotoThet quasi fini, essayez d'y jeter un coup d'oeil pour me donner vos impressions

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 2:abdf8c6823a1 5 Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc)
sype 0:ad9600df4a70 6 {
sype 0:ad9600df4a70 7 m_v = v;
sype 0:ad9600df4a70 8 m_distPerTick_left = diameter_left*PI/37400;
sype 0:ad9600df4a70 9 m_distPerTick_right = diameter_right*PI/37400;
sype 0:ad9600df4a70 10
sype 3:62e9d715de65 11 roboclaw.ForwardM1(ADR, 0);
sype 3:62e9d715de65 12 roboclaw.ForwardM2(ADR, 0);
sype 3:62e9d715de65 13
sype 3:62e9d715de65 14 erreur_ang = 0.01;
sype 0:ad9600df4a70 15 m_pulses_right = 0;
sype 0:ad9600df4a70 16 m_pulses_left = 0;
sype 2:abdf8c6823a1 17 wait_ms(100);
sype 0:ad9600df4a70 18 }
sype 0:ad9600df4a70 19
sype 0:ad9600df4a70 20 void Odometry::setPos(double x, double y, double theta)
sype 0:ad9600df4a70 21 {
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 0:ad9600df4a70 27 void Odometry::setX(double x)
sype 0:ad9600df4a70 28 {
sype 0:ad9600df4a70 29 this->x = x;
sype 0:ad9600df4a70 30 }
sype 0:ad9600df4a70 31
sype 0:ad9600df4a70 32 void Odometry::setY(double y)
sype 0:ad9600df4a70 33 {
sype 0:ad9600df4a70 34 this->y = y;
sype 0:ad9600df4a70 35 }
sype 0:ad9600df4a70 36
sype 0:ad9600df4a70 37 void Odometry::setTheta(double theta)
sype 0:ad9600df4a70 38 {
sype 0:ad9600df4a70 39 this->theta = theta;
sype 0:ad9600df4a70 40 }
sype 0:ad9600df4a70 41
sype 0:ad9600df4a70 42 void Odometry::update_odo(void)
sype 0:ad9600df4a70 43 {
sype 3:62e9d715de65 44 int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
sype 0:ad9600df4a70 45 m_pulses_right = roboclaw.ReadEncM1(ADR);
sype 3:62e9d715de65 46 int32_t delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
sype 0:ad9600df4a70 47 m_pulses_left = roboclaw.ReadEncM2(ADR);
sype 0:ad9600df4a70 48
sype 0:ad9600df4a70 49 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
sype 2:abdf8c6823a1 50 double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
sype 0:ad9600df4a70 51
sype 2:abdf8c6823a1 52 double radius = deltaS/deltaTheta;
sype 2:abdf8c6823a1 53 double xO = x - radius*sin(theta);
sype 2:abdf8c6823a1 54 double yO = y + radius*cos(theta);
sype 0:ad9600df4a70 55
sype 0:ad9600df4a70 56 theta += deltaTheta;
sype 0:ad9600df4a70 57
sype 2:abdf8c6823a1 58 x = xO + radius*sin(theta);
sype 2:abdf8c6823a1 59 y = yO - radius*cos(theta);
sype 2:abdf8c6823a1 60
sype 0:ad9600df4a70 61 while(theta > PI) theta -= 2*PI;
sype 0:ad9600df4a70 62 while(theta <= -PI) theta += 2*PI;
sype 0:ad9600df4a70 63 }
sype 0:ad9600df4a70 64
sype 0:ad9600df4a70 65 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
sype 0:ad9600df4a70 66 {
sype 0:ad9600df4a70 67 double theta_ = atan2(y_goal-y, x_goal-x);
sype 3:62e9d715de65 68 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 2:abdf8c6823a1 69 GotoThet(theta_);
sype 3:62e9d715de65 70 GotoDist(dist_);
sype 0:ad9600df4a70 71 }
sype 2:abdf8c6823a1 72
sype 2:abdf8c6823a1 73 void Odometry::GotoThet(double theta_)
sype 2:abdf8c6823a1 74 {
sype 2:abdf8c6823a1 75 double distance_ticks_left;
sype 2:abdf8c6823a1 76 double distance_ticks_right;
sype 3:62e9d715de65 77 double erreur_theta = theta_ - getTheta();
sype 3:62e9d715de65 78 bool arrived = false;
sype 2:abdf8c6823a1 79
sype 3:62e9d715de65 80 while(erreur_theta >= PI) erreur_theta -= 2*PI;
sype 2:abdf8c6823a1 81 while(erreur_theta <= -PI) erreur_theta += 2*PI;
sype 2:abdf8c6823a1 82
sype 3:62e9d715de65 83 if(erreur_theta <= 0)
sype 2:abdf8c6823a1 84 {
sype 2:abdf8c6823a1 85 distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
sype 2:abdf8c6823a1 86 distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
sype 2:abdf8c6823a1 87 }
sype 2:abdf8c6823a1 88 else
sype 2:abdf8c6823a1 89 {
sype 2:abdf8c6823a1 90 distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
sype 2:abdf8c6823a1 91 distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
sype 2:abdf8c6823a1 92 }
sype 3:62e9d715de65 93 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 94 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 100000, (int32_t)distance_ticks_right, 150000, 150000, 100000, (int32_t)distance_ticks_left, 1);
sype 2:abdf8c6823a1 95 }
sype 2:abdf8c6823a1 96
sype 3:62e9d715de65 97 void Odometry::GotoDist(double distance)
sype 2:abdf8c6823a1 98 {
sype 3:62e9d715de65 99 double temp1 = roboclaw.ReadEncM1(ADR), temp2 = roboclaw.ReadEncM2(ADR);
sype 3:62e9d715de65 100 double distance_ticks_left = distance/m_distPerTick_left - temp2;
sype 3:62e9d715de65 101 double distance_ticks_right = distance/m_distPerTick_right - temp1;
sype 3:62e9d715de65 102 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (int32_t)distance_ticks_right, 150000, 200000, 150000, (int32_t)distance_ticks_left, 1);
sype 2:abdf8c6823a1 103 }
sype 2:abdf8c6823a1 104
sype 2:abdf8c6823a1 105 bool Odometry::isArrivedRot(double theta_)
sype 2:abdf8c6823a1 106 {
sype 3:62e9d715de65 107 if(abs_d(getTheta()) <= abs_d(theta_+erreur_ang)) return true;
sype 3:62e9d715de65 108 else if(abs_d(getTheta()) >= abs_d(theta_-erreur_ang)) return true;
sype 2:abdf8c6823a1 109 else return false;
sype 2:abdf8c6823a1 110 }