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@10:ae3178aa94e9, 2015-12-04 (annotated)

Committer:
sype
Date:
Fri Dec 04 11:18:13 2015 +0000
Revision:
10:ae3178aa94e9
Parent:
4:3e6e78d6d3d9
Child:
14:d5e21f71c2a9
80% Motorisation

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 10:ae3178aa94e9 5 Odometry::Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
sype 10:ae3178aa94e9 6 {
sype 0:ad9600df4a70 7 m_v = v;
sype 10:ae3178aa94e9 8 m_distPerTick_left = diameter_left*PI/quadrature;
sype 10:ae3178aa94e9 9 m_distPerTick_right = diameter_right*PI/quadrature;
sype 4:3e6e78d6d3d9 10
sype 3:62e9d715de65 11 roboclaw.ForwardM1(ADR, 0);
sype 3:62e9d715de65 12 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 13 roboclaw.ResetEnc(ADR);
sype 4:3e6e78d6d3d9 14 // Erreur autorisée sur le déplacement en angle
sype 3:62e9d715de65 15 erreur_ang = 0.01;
sype 4:3e6e78d6d3d9 16
sype 0:ad9600df4a70 17 m_pulses_right = 0;
sype 0:ad9600df4a70 18 m_pulses_left = 0;
sype 10:ae3178aa94e9 19 pos_prog = 0;
sype 2:abdf8c6823a1 20 wait_ms(100);
sype 0:ad9600df4a70 21 }
sype 0:ad9600df4a70 22
sype 10:ae3178aa94e9 23 void Odometry::setPos(double x, double y, double theta)
sype 10:ae3178aa94e9 24 {
sype 0:ad9600df4a70 25 this->x = x;
sype 0:ad9600df4a70 26 this->y = y;
sype 0:ad9600df4a70 27 this->theta = theta;
sype 0:ad9600df4a70 28 }
sype 0:ad9600df4a70 29
sype 10:ae3178aa94e9 30 void Odometry::setX(double x)
sype 10:ae3178aa94e9 31 {
sype 0:ad9600df4a70 32 this->x = x;
sype 0:ad9600df4a70 33 }
sype 0:ad9600df4a70 34
sype 10:ae3178aa94e9 35 void Odometry::setY(double y)
sype 10:ae3178aa94e9 36 {
sype 0:ad9600df4a70 37 this->y = y;
sype 0:ad9600df4a70 38 }
sype 0:ad9600df4a70 39
sype 10:ae3178aa94e9 40 void Odometry::setTheta(double theta)
sype 10:ae3178aa94e9 41 {
sype 0:ad9600df4a70 42 this->theta = theta;
sype 0:ad9600df4a70 43 }
sype 0:ad9600df4a70 44
sype 10:ae3178aa94e9 45 void Odometry::update_odo(void)
sype 10:ae3178aa94e9 46 {
sype 3:62e9d715de65 47 int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
sype 0:ad9600df4a70 48 m_pulses_right = roboclaw.ReadEncM1(ADR);
sype 3:62e9d715de65 49 int32_t delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
sype 0:ad9600df4a70 50 m_pulses_left = roboclaw.ReadEncM2(ADR);
sype 4:3e6e78d6d3d9 51
sype 0:ad9600df4a70 52 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
sype 10:ae3178aa94e9 53 double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right) / m_v;
sype 4:3e6e78d6d3d9 54
sype 10:ae3178aa94e9 55 double R = deltaS/deltaTheta;
sype 10:ae3178aa94e9 56
sype 10:ae3178aa94e9 57 double xO = x - R*sin(theta);
sype 10:ae3178aa94e9 58 double yO = y + R*cos(theta);
sype 4:3e6e78d6d3d9 59
sype 0:ad9600df4a70 60 theta += deltaTheta;
sype 4:3e6e78d6d3d9 61
sype 10:ae3178aa94e9 62 if(deltaTheta == 0) {
sype 10:ae3178aa94e9 63 x = x + deltaS*cos(theta);
sype 10:ae3178aa94e9 64 y = y + deltaS*sin(theta);
sype 10:ae3178aa94e9 65 }
sype 10:ae3178aa94e9 66 else {
sype 10:ae3178aa94e9 67 x = xO + R*sin(theta);
sype 10:ae3178aa94e9 68 y = yO - R*cos(theta);
sype 10:ae3178aa94e9 69 }
sype 10:ae3178aa94e9 70
sype 10:ae3178aa94e9 71 /*double dx = deltaS*cos(theta);
sype 10:ae3178aa94e9 72 double dy = deltaS*sin(theta);
sype 10:ae3178aa94e9 73 x += dx;
sype 10:ae3178aa94e9 74 y += dy;
sype 10:ae3178aa94e9 75 theta += deltaTheta;*/
sype 4:3e6e78d6d3d9 76
sype 0:ad9600df4a70 77 while(theta > PI) theta -= 2*PI;
sype 0:ad9600df4a70 78 while(theta <= -PI) theta += 2*PI;
sype 0:ad9600df4a70 79 }
sype 0:ad9600df4a70 80
sype 10:ae3178aa94e9 81 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
sype 10:ae3178aa94e9 82 {
sype 0:ad9600df4a70 83 double theta_ = atan2(y_goal-y, x_goal-x);
sype 3:62e9d715de65 84 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 10:ae3178aa94e9 85 //pc.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
sype 10:ae3178aa94e9 86 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
sype 2:abdf8c6823a1 87 GotoThet(theta_);
sype 3:62e9d715de65 88 GotoDist(dist_);
sype 0:ad9600df4a70 89 }
sype 2:abdf8c6823a1 90
sype 10:ae3178aa94e9 91 void Odometry::GotoThet(double theta_)
sype 10:ae3178aa94e9 92 {
sype 10:ae3178aa94e9 93 led = 0;
sype 10:ae3178aa94e9 94 //pos_prog++;
sype 10:ae3178aa94e9 95 //pc.printf("Theta : %3.2f\n\r", theta_*180/PI);
sype 10:ae3178aa94e9 96 //arrived = false;
sype 10:ae3178aa94e9 97
sype 10:ae3178aa94e9 98 int32_t distance_ticks_left;
sype 10:ae3178aa94e9 99 int32_t distance_ticks_right;
sype 10:ae3178aa94e9 100
sype 10:ae3178aa94e9 101 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
sype 4:3e6e78d6d3d9 102
sype 4:3e6e78d6d3d9 103 // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
sype 3:62e9d715de65 104 double erreur_theta = theta_ - getTheta();
sype 4:3e6e78d6d3d9 105
sype 3:62e9d715de65 106 while(erreur_theta >= PI) erreur_theta -= 2*PI;
sype 2:abdf8c6823a1 107 while(erreur_theta <= -PI) erreur_theta += 2*PI;
sype 4:3e6e78d6d3d9 108
sype 4:3e6e78d6d3d9 109 if(erreur_theta <= 0) {
sype 10:ae3178aa94e9 110 distance_ticks_left = (int32_t) -(erreur_theta*m_v/2)/m_distPerTick_left + pos_initiale_left;
sype 10:ae3178aa94e9 111 distance_ticks_right = (int32_t) (erreur_theta*m_v/2)/m_distPerTick_right + pos_initiale_right;
sype 4:3e6e78d6d3d9 112 } else {
sype 10:ae3178aa94e9 113 distance_ticks_left = (int32_t) (erreur_theta*m_v/2)/m_distPerTick_left + pos_initiale_left;
sype 10:ae3178aa94e9 114 distance_ticks_right = (int32_t) -(erreur_theta*m_v/2)/m_distPerTick_right + pos_initiale_right;
sype 2:abdf8c6823a1 115 }
sype 10:ae3178aa94e9 116
sype 10:ae3178aa94e9 117 //pc.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
sype 3:62e9d715de65 118 pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
sype 10:ae3178aa94e9 119 //pc.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
sype 10:ae3178aa94e9 120
sype 10:ae3178aa94e9 121 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
sype 10:ae3178aa94e9 122
sype 10:ae3178aa94e9 123 //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
sype 10:ae3178aa94e9 124
sype 10:ae3178aa94e9 125 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);
sype 10:ae3178aa94e9 126 //setTheta(theta_);
sype 10:ae3178aa94e9 127 led = 1;
sype 10:ae3178aa94e9 128 //arrived = true;
sype 10:ae3178aa94e9 129 //pc.printf("arrivey %d\n\r",pos_prog);
sype 2:abdf8c6823a1 130 }
sype 2:abdf8c6823a1 131
sype 10:ae3178aa94e9 132 void Odometry::GotoDist(double distance)
sype 10:ae3178aa94e9 133 {
sype 10:ae3178aa94e9 134 led = 0;
sype 10:ae3178aa94e9 135 //pos_prog++;
sype 10:ae3178aa94e9 136 //pc.printf("Dist : %3.2f\n\r", distance);
sype 10:ae3178aa94e9 137 //arrived = false;
sype 10:ae3178aa94e9 138
sype 10:ae3178aa94e9 139 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
sype 10:ae3178aa94e9 140
sype 10:ae3178aa94e9 141 int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
sype 10:ae3178aa94e9 142 int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
sype 10:ae3178aa94e9 143
sype 10:ae3178aa94e9 144 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
sype 10:ae3178aa94e9 145
sype 10:ae3178aa94e9 146 //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
sype 10:ae3178aa94e9 147
sype 10:ae3178aa94e9 148 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);
sype 10:ae3178aa94e9 149
sype 10:ae3178aa94e9 150 led = 1;
sype 10:ae3178aa94e9 151 //pc.printf("arrivey %d\n\r",pos_prog);
sype 10:ae3178aa94e9 152 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
sype 2:abdf8c6823a1 153 }