ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Odometry/Odometry.cpp@77:f19cc7f81f2a, 2016-05-05 (annotated)

Committer:
sype
Date:
Thu May 05 08:46:08 2016 +0000
Revision:
77:f19cc7f81f2a
Parent:
49:5e2f7323f280 
commit homologation

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