ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

Odometry/Odometry.cpp@41:53d5990ff99d, 2016-04-13 (annotated)

Committer:
IceTeam
Date:
Wed Apr 13 16:17:19 2016 +0000
Revision:
41:53d5990ff99d
Parent:
39:ca4dd3faffa8 
Correction odometrie;

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 14:d5e21f71c2a9 29 void Odometry::getEnc()
sype 14:d5e21f71c2a9 30 {
sype 38:da3a2c781672 31 logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR));
sype 14:d5e21f71c2a9 32 }
sype 0:ad9600df4a70 33
sype 10:ae3178aa94e9 34 void Odometry::setX(double x)
sype 10:ae3178aa94e9 35 {
sype 0:ad9600df4a70 36 this->x = x;
sype 0:ad9600df4a70 37 }
sype 0:ad9600df4a70 38
sype 10:ae3178aa94e9 39 void Odometry::setY(double y)
sype 10:ae3178aa94e9 40 {
sype 0:ad9600df4a70 41 this->y = y;
sype 0:ad9600df4a70 42 }
sype 0:ad9600df4a70 43
sype 10:ae3178aa94e9 44 void Odometry::setTheta(double theta)
sype 10:ae3178aa94e9 45 {
sype 0:ad9600df4a70 46 this->theta = theta;
sype 0:ad9600df4a70 47 }
sype 0:ad9600df4a70 48
sype 10:ae3178aa94e9 49 void Odometry::update_odo(void)
sype 10:ae3178aa94e9 50 {
Near32 32:068bd2b2e1f3 51 int32_t roboclawENCM1 = roboclaw.ReadEncM1(ADR);
Near32 32:068bd2b2e1f3 52 int32_t roboclawENCM2 = roboclaw.ReadEncM2(ADR);
Near32 32:068bd2b2e1f3 53 int32_t delta_right = roboclawENCM1 - m_pulses_right;
Near32 32:068bd2b2e1f3 54 m_pulses_right = roboclawENCM1;
Near32 32:068bd2b2e1f3 55 int32_t delta_left = roboclawENCM2 - m_pulses_left;
Near32 32:068bd2b2e1f3 56 m_pulses_left = roboclawENCM2;
sype 14:d5e21f71c2a9 57
sype 14:d5e21f71c2a9 58 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right)*C / 2.0f;
IceTeam 41:53d5990ff99d 59 // deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
IceTeam 41:53d5990ff99d 60 double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
sype 4:3e6e78d6d3d9 61
sype 14:d5e21f71c2a9 62 /*double R = deltaS/deltaTheta;
sype 10:ae3178aa94e9 63
sype 10:ae3178aa94e9 64 double xO = x - R*sin(theta);
sype 10:ae3178aa94e9 65 double yO = y + R*cos(theta);
sype 4:3e6e78d6d3d9 66
sype 0:ad9600df4a70 67 theta += deltaTheta;
sype 4:3e6e78d6d3d9 68
sype 10:ae3178aa94e9 69 if(deltaTheta == 0) {
sype 10:ae3178aa94e9 70 x = x + deltaS*cos(theta);
sype 10:ae3178aa94e9 71 y = y + deltaS*sin(theta);
sype 10:ae3178aa94e9 72 }
sype 10:ae3178aa94e9 73 else {
sype 10:ae3178aa94e9 74 x = xO + R*sin(theta);
sype 10:ae3178aa94e9 75 y = yO - R*cos(theta);
sype 14:d5e21f71c2a9 76 }*/
IceTeam 36:2d7357a385bc 77
IceTeam 36:2d7357a385bc 78
IceTeam 36:2d7357a385bc 79
IceTeam 36:2d7357a385bc 80 double dx = deltaS*cos(theta+deltaTheta/2);
IceTeam 36:2d7357a385bc 81 double dy = deltaS*sin(theta+deltaTheta/2);
IceTeam 36:2d7357a385bc 82 x += dx;
IceTeam 36:2d7357a385bc 83 y += dy;
IceTeam 36:2d7357a385bc 84
Near32 34:7f8c29ddee61 85 theta += deltaTheta;
IceTeam 41:53d5990ff99d 86 // theta = (theta%(2*PI))-PI;
Near32 34:7f8c29ddee61 87 while(theta > PI) theta -= 2*PI;
Near32 34:7f8c29ddee61 88 while(theta <= -PI) theta += 2*PI;
Near32 34:7f8c29ddee61 89
sype 0:ad9600df4a70 90 }
sype 0:ad9600df4a70 91
sype 14:d5e21f71c2a9 92 void Odometry::GotoXY(double x_goal, double y_goal)
sype 14:d5e21f71c2a9 93 {
IceTeam 41:53d5990ff99d 94 double theta_;
IceTeam 41:53d5990ff99d 95 if (x_goal-x != 0)
IceTeam 41:53d5990ff99d 96 theta_ = atan2(y_goal-y, x_goal-x);
IceTeam 41:53d5990ff99d 97 else
IceTeam 41:53d5990ff99d 98 if (y_goal - y >= 0)
IceTeam 41:53d5990ff99d 99 theta_ = PI/2;
IceTeam 41:53d5990ff99d 100 else
IceTeam 41:53d5990ff99d 101 theta_ = -PI/2;
IceTeam 41:53d5990ff99d 102
sype 14:d5e21f71c2a9 103 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 14:d5e21f71c2a9 104 GotoThet(theta_);
sype 14:d5e21f71c2a9 105 GotoDist(dist_);
sype 14:d5e21f71c2a9 106 }
sype 14:d5e21f71c2a9 107
sype 10:ae3178aa94e9 108 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
sype 10:ae3178aa94e9 109 {
sype 0:ad9600df4a70 110 double theta_ = atan2(y_goal-y, x_goal-x);
sype 3:62e9d715de65 111 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
IceTeam 39:ca4dd3faffa8 112 //logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
sype 2:abdf8c6823a1 113 GotoThet(theta_);
sype 3:62e9d715de65 114 GotoDist(dist_);
sype 14:d5e21f71c2a9 115 GotoThet(theta_goal);
sype 0:ad9600df4a70 116 }
sype 2:abdf8c6823a1 117
sype 10:ae3178aa94e9 118 void Odometry::GotoThet(double theta_)
sype 10:ae3178aa94e9 119 {
sype 10:ae3178aa94e9 120 led = 0;
sype 10:ae3178aa94e9 121 //pos_prog++;
sype 38:da3a2c781672 122 //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
sype 10:ae3178aa94e9 123 //arrived = false;
sype 10:ae3178aa94e9 124 int32_t distance_ticks_left;
sype 10:ae3178aa94e9 125 int32_t distance_ticks_right;
sype 10:ae3178aa94e9 126
sype 10:ae3178aa94e9 127 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
sype 4:3e6e78d6d3d9 128
sype 4:3e6e78d6d3d9 129 // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
sype 3:62e9d715de65 130 double erreur_theta = theta_ - getTheta();
sype 4:3e6e78d6d3d9 131
sype 3:62e9d715de65 132 while(erreur_theta >= PI) erreur_theta -= 2*PI;
sype 14:d5e21f71c2a9 133 while(erreur_theta < -PI) erreur_theta += 2*PI;
sype 14:d5e21f71c2a9 134
IceTeam 41:53d5990ff99d 135 //logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
sype 14:d5e21f71c2a9 136
sype 14:d5e21f71c2a9 137 if(erreur_theta < 0) {
IceTeam 41:53d5990ff99d 138 distance_ticks_left = (int32_t) pos_initiale_left - (erreur_theta*m_v/2)/m_distPerTick_left;
IceTeam 41:53d5990ff99d 139 distance_ticks_right = (int32_t) pos_initiale_right + (erreur_theta*m_v/2)/m_distPerTick_right;
IceTeam 41:53d5990ff99d 140
sype 4:3e6e78d6d3d9 141 } else {
IceTeam 41:53d5990ff99d 142 distance_ticks_left = (int32_t) pos_initiale_left - (erreur_theta*m_v/2)/m_distPerTick_left;
IceTeam 41:53d5990ff99d 143 distance_ticks_right = (int32_t) pos_initiale_right + (erreur_theta*m_v/2)/m_distPerTick_right;
sype 2:abdf8c6823a1 144 }
sype 10:ae3178aa94e9 145
sype 38:da3a2c781672 146 //logger.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
sype 38:da3a2c781672 147 //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
sype 38:da3a2c781672 148 //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
sype 10:ae3178aa94e9 149
sype 10:ae3178aa94e9 150 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 39:ca4dd3faffa8 151
IceTeam 41:53d5990ff99d 152 //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
IceTeam 39:ca4dd3faffa8 153
IceTeam 41:53d5990ff99d 154 /*while ((m_pulses_right - distance_ticks_right > 0 ? m_pulses_right - distance_ticks_right : distance_ticks_right - m_pulses_right) > 1 &&
IceTeam 41:53d5990ff99d 155 (m_pulses_left - distance_ticks_left > 0 ? m_pulses_left - distance_ticks_left : distance_ticks_left - m_pulses_left) > 1)
IceTeam 41:53d5990ff99d 156 logger.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);
IceTeam 41:53d5990ff99d 157
IceTeam 41:53d5990ff99d 158 while(((m_pulses_right < distance_ticks_right-1) || (m_pulses_left > distance_ticks_left+1)) && sens ||
IceTeam 41:53d5990ff99d 159 ((m_pulses_right > distance_ticks_right+1) || (m_pulses_left < distance_ticks_left-1)) && (!sens))
IceTeam 41:53d5990ff99d 160 logger.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);
IceTeam 41:53d5990ff99d 161 */
IceTeam 41:53d5990ff99d 162
IceTeam 41:53d5990ff99d 163 while (m_pulses_right != distance_ticks_right && m_pulses_left != distance_ticks_left);
IceTeam 41:53d5990ff99d 164 //logger.printf("[%f-%f] Theta : %f (theta %f-%f)\n\r", x, y, theta, m_pulses_right-distance_ticks_right, m_pulses_left-distance_ticks_left);
IceTeam 41:53d5990ff99d 165
IceTeam 31:8bcc3a0bfa8a 166 wait(0.4);
IceTeam 41:53d5990ff99d 167 //setTheta(theta_);
sype 10:ae3178aa94e9 168 led = 1;
sype 10:ae3178aa94e9 169 //arrived = true;
sype 38:da3a2c781672 170 //logger.printf("arrivey %d\n\r",pos_prog);
sype 2:abdf8c6823a1 171 }
sype 2:abdf8c6823a1 172
sype 10:ae3178aa94e9 173 void Odometry::GotoDist(double distance)
sype 10:ae3178aa94e9 174 {
sype 10:ae3178aa94e9 175 led = 0;
sype 10:ae3178aa94e9 176 //pos_prog++;
sype 38:da3a2c781672 177 //logger.printf("Dist : %3.2f\n\r", distance);
sype 10:ae3178aa94e9 178 //arrived = false;
sype 10:ae3178aa94e9 179
sype 10:ae3178aa94e9 180 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
sype 10:ae3178aa94e9 181
sype 10:ae3178aa94e9 182 int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
sype 10:ae3178aa94e9 183 int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
sype 10:ae3178aa94e9 184
sype 10:ae3178aa94e9 185 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 186
sype 38:da3a2c781672 187 //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
sype 14:d5e21f71c2a9 188
IceTeam 41:53d5990ff99d 189 while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
IceTeam 41:53d5990ff99d 190 //logger.printf("[%f-%f] Theta : %f (dist %f-%f)\n\r", x, y, theta, m_pulses_right-distance_ticks_right, m_pulses_left-distance_ticks_left);
IceTeam 41:53d5990ff99d 191
IceTeam 31:8bcc3a0bfa8a 192 wait(0.4);
sype 10:ae3178aa94e9 193 led = 1;
sype 38:da3a2c781672 194 //logger.printf("arrivey %d\n\r",pos_prog);
sype 38:da3a2c781672 195 //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
sype 2:abdf8c6823a1 196 }
IceTeam 35:4e3d9ab1b94b 197
IceTeam 35:4e3d9ab1b94b 198 void Odometry::TestEntraxe(int i) {
IceTeam 35:4e3d9ab1b94b 199 int32_t distance_ticks_left;
IceTeam 35:4e3d9ab1b94b 200 int32_t distance_ticks_right;
IceTeam 35:4e3d9ab1b94b 201
IceTeam 35:4e3d9ab1b94b 202 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
IceTeam 35:4e3d9ab1b94b 203
IceTeam 35:4e3d9ab1b94b 204 double erreur_theta = 2*PI*i - getTheta();
IceTeam 35:4e3d9ab1b94b 205 if(erreur_theta < 0) {
IceTeam 35:4e3d9ab1b94b 206 distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
IceTeam 35:4e3d9ab1b94b 207 distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
IceTeam 35:4e3d9ab1b94b 208 } else {
IceTeam 35:4e3d9ab1b94b 209 distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
IceTeam 35:4e3d9ab1b94b 210 distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
IceTeam 35:4e3d9ab1b94b 211 }
IceTeam 35:4e3d9ab1b94b 212
IceTeam 35:4e3d9ab1b94b 213 roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 35:4e3d9ab1b94b 214
IceTeam 35:4e3d9ab1b94b 215 while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
IceTeam 35:4e3d9ab1b94b 216 wait(0.4);
IceTeam 35:4e3d9ab1b94b 217 }