Simon Emarre / Mbed 2 deprecated RoboClaw

main

Dependencies:   RoboClaw mbed

Odometry/Odometry.cpp@2:abdf8c6823a1, 2015-11-24 (annotated)

Committer:
sype
Date:
Tue Nov 24 15:02:01 2015 +0000
Revision:
2:abdf8c6823a1
Parent:
0:ad9600df4a70
Child:
3:62e9d715de65
gotothet

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 2:abdf8c6823a1 11 erreur_ang = 0.1;
sype 0:ad9600df4a70 12 m_pulses_right = 0;
sype 0:ad9600df4a70 13 m_pulses_left = 0;
sype 2:abdf8c6823a1 14 wait_ms(100);
sype 0:ad9600df4a70 15 }
sype 0:ad9600df4a70 16
sype 0:ad9600df4a70 17 void Odometry::setPos(double x, double y, double theta)
sype 0:ad9600df4a70 18 {
sype 0:ad9600df4a70 19 this->x = x;
sype 0:ad9600df4a70 20 this->y = y;
sype 0:ad9600df4a70 21 this->theta = theta;
sype 0:ad9600df4a70 22 }
sype 0:ad9600df4a70 23
sype 0:ad9600df4a70 24 void Odometry::setX(double x)
sype 0:ad9600df4a70 25 {
sype 0:ad9600df4a70 26 this->x = x;
sype 0:ad9600df4a70 27 }
sype 0:ad9600df4a70 28
sype 0:ad9600df4a70 29 void Odometry::setY(double y)
sype 0:ad9600df4a70 30 {
sype 0:ad9600df4a70 31 this->y = y;
sype 0:ad9600df4a70 32 }
sype 0:ad9600df4a70 33
sype 0:ad9600df4a70 34 void Odometry::setTheta(double theta)
sype 0:ad9600df4a70 35 {
sype 0:ad9600df4a70 36 this->theta = theta;
sype 0:ad9600df4a70 37 }
sype 0:ad9600df4a70 38
sype 0:ad9600df4a70 39 void Odometry::update_odo(void)
sype 0:ad9600df4a70 40 {
sype 0:ad9600df4a70 41 long delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
sype 0:ad9600df4a70 42 m_pulses_right = roboclaw.ReadEncM1(ADR);
sype 0:ad9600df4a70 43 long delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
sype 0:ad9600df4a70 44 m_pulses_left = roboclaw.ReadEncM2(ADR);
sype 0:ad9600df4a70 45
sype 0:ad9600df4a70 46 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
sype 2:abdf8c6823a1 47 double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
sype 0:ad9600df4a70 48
sype 2:abdf8c6823a1 49 double radius = deltaS/deltaTheta;
sype 2:abdf8c6823a1 50 double xO = x - radius*sin(theta);
sype 2:abdf8c6823a1 51 double yO = y + radius*cos(theta);
sype 0:ad9600df4a70 52
sype 0:ad9600df4a70 53 theta += deltaTheta;
sype 0:ad9600df4a70 54
sype 2:abdf8c6823a1 55 x = xO + radius*sin(theta);
sype 2:abdf8c6823a1 56 y = yO - radius*cos(theta);
sype 2:abdf8c6823a1 57
sype 0:ad9600df4a70 58 while(theta > PI) theta -= 2*PI;
sype 0:ad9600df4a70 59 while(theta <= -PI) theta += 2*PI;
sype 0:ad9600df4a70 60 }
sype 0:ad9600df4a70 61
sype 0:ad9600df4a70 62 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
sype 0:ad9600df4a70 63 {
sype 0:ad9600df4a70 64 double theta_ = atan2(y_goal-y, x_goal-x);
sype 2:abdf8c6823a1 65 float distance = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 2:abdf8c6823a1 66 GotoThet(theta_);
sype 0:ad9600df4a70 67 }
sype 2:abdf8c6823a1 68
sype 2:abdf8c6823a1 69 void Odometry::GotoThet(double theta_)
sype 2:abdf8c6823a1 70 {
sype 2:abdf8c6823a1 71 double distance_ticks_left;
sype 2:abdf8c6823a1 72 double distance_ticks_right;
sype 2:abdf8c6823a1 73 double erreur_theta = theta_-getTheta();
sype 2:abdf8c6823a1 74
sype 2:abdf8c6823a1 75 while(erreur_theta > PI) erreur_theta -= 2*PI;
sype 2:abdf8c6823a1 76 while(erreur_theta <= -PI) erreur_theta += 2*PI;
sype 2:abdf8c6823a1 77
sype 2:abdf8c6823a1 78 if(erreur_theta >= 0)
sype 2:abdf8c6823a1 79 {
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 2:abdf8c6823a1 82 }
sype 2:abdf8c6823a1 83 else
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
sype 2:abdf8c6823a1 89 pc.printf("T_%3.2f\t T%3.2f\t ET%3.2f\n\r",theta_*180/PI, getTheta()*180/PI, erreur_theta*180/PI);
sype 2:abdf8c6823a1 90 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 150000, (long)distance_ticks_right, 150000, 150000, 150000, (long)distance_ticks_left, 1);
sype 2:abdf8c6823a1 91 while(isArrivedRot(erreur_theta))pc.printf("Theta : %3.2f\n\r",getTheta()*180/PI);;
sype 2:abdf8c6823a1 92 pc.printf("Arrived");
sype 2:abdf8c6823a1 93 }
sype 2:abdf8c6823a1 94
sype 2:abdf8c6823a1 95 void Odometry::GotoB(double distance)
sype 2:abdf8c6823a1 96 {
sype 2:abdf8c6823a1 97 double distance_ticks_left = distance/m_distPerTick_left;
sype 2:abdf8c6823a1 98 double distance_ticks_right = distance/m_distPerTick_right;
sype 2:abdf8c6823a1 99 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (long)distance_ticks_right, 150000, 200000, 150000, (long)distance_ticks_left, 1);
sype 2:abdf8c6823a1 100 }
sype 2:abdf8c6823a1 101
sype 2:abdf8c6823a1 102 bool Odometry::isArrivedRot(double theta_)
sype 2:abdf8c6823a1 103 {
sype 2:abdf8c6823a1 104 if((abs_d(getTheta())<=abs_d(theta_)+erreur_ang)|(abs_d(getTheta())>=abs_d(theta_)-erreur_ang)) return true;
sype 2:abdf8c6823a1 105 else return false;
sype 2:abdf8c6823a1 106 }