Romain Ame / Mbed 2 deprecated Timer71pt

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   RoboClaw mbed

Fork of Timer by ARES

Odometry/Odometry.cpp@78:c28bdbf29b6e, 2016-05-05 (annotated)

Committer:
IceTeam
Date:
Thu May 05 13:17:00 2016 +0000
Revision:
78:c28bdbf29b6e
Parent:
77:f19cc7f81f2a 
Des trucs qui compilent

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;
IceTeam 78:c28bdbf29b6e 20
IceTeam 78:c28bdbf29b6e 21 paused = false;
IceTeam 78:c28bdbf29b6e 22
IceTeam 78:c28bdbf29b6e 23 timer.stop();
IceTeam 78:c28bdbf29b6e 24 timer.reset();
IceTeam 78:c28bdbf29b6e 25
sype 2:abdf8c6823a1 26 wait_ms(100);
sype 0:ad9600df4a70 27 }
sype 0:ad9600df4a70 28
sype 10:ae3178aa94e9 29 void Odometry::setPos(double x, double y, double theta)
sype 10:ae3178aa94e9 30 {
sype 0:ad9600df4a70 31 this->x = x;
sype 0:ad9600df4a70 32 this->y = y;
sype 0:ad9600df4a70 33 this->theta = theta;
sype 0:ad9600df4a70 34 }
sype 12:d5e21f71c2a9 35 void Odometry::getEnc()
sype 12:d5e21f71c2a9 36 {
sype 46:5658af4e5149 37 logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(), roboclaw.ReadEncM2());
sype 12:d5e21f71c2a9 38 }
sype 0:ad9600df4a70 39
sype 10:ae3178aa94e9 40 void Odometry::setX(double x)
sype 10:ae3178aa94e9 41 {
sype 0:ad9600df4a70 42 this->x = x;
sype 0:ad9600df4a70 43 }
sype 0:ad9600df4a70 44
sype 10:ae3178aa94e9 45 void Odometry::setY(double y)
sype 10:ae3178aa94e9 46 {
sype 0:ad9600df4a70 47 this->y = y;
sype 0:ad9600df4a70 48 }
sype 0:ad9600df4a70 49
sype 10:ae3178aa94e9 50 void Odometry::setTheta(double theta)
sype 10:ae3178aa94e9 51 {
sype 0:ad9600df4a70 52 this->theta = theta;
sype 0:ad9600df4a70 53 }
sype 0:ad9600df4a70 54
sype 10:ae3178aa94e9 55 void Odometry::update_odo(void)
sype 10:ae3178aa94e9 56 {
sype 46:5658af4e5149 57 int32_t roboclawENCM1 = roboclaw.ReadEncM1();
sype 46:5658af4e5149 58 int32_t roboclawENCM2 = roboclaw.ReadEncM2();
Near32 32:068bd2b2e1f3 59 int32_t delta_right = roboclawENCM1 - m_pulses_right;
Near32 32:068bd2b2e1f3 60 m_pulses_right = roboclawENCM1;
Near32 32:068bd2b2e1f3 61 int32_t delta_left = roboclawENCM2 - m_pulses_left;
Near32 32:068bd2b2e1f3 62 m_pulses_left = roboclawENCM2;
sype 12:d5e21f71c2a9 63
sype 12:d5e21f71c2a9 64 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right)*C / 2.0f;
sype 12:d5e21f71c2a9 65 double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
sype 4:3e6e78d6d3d9 66
sype 12:d5e21f71c2a9 67 /*double R = deltaS/deltaTheta;
sype 10:ae3178aa94e9 68
sype 10:ae3178aa94e9 69 double xO = x - R*sin(theta);
sype 10:ae3178aa94e9 70 double yO = y + R*cos(theta);
sype 4:3e6e78d6d3d9 71
sype 0:ad9600df4a70 72 theta += deltaTheta;
sype 4:3e6e78d6d3d9 73
sype 10:ae3178aa94e9 74 if(deltaTheta == 0) {
sype 10:ae3178aa94e9 75 x = x + deltaS*cos(theta);
sype 10:ae3178aa94e9 76 y = y + deltaS*sin(theta);
sype 10:ae3178aa94e9 77 }
sype 10:ae3178aa94e9 78 else {
sype 10:ae3178aa94e9 79 x = xO + R*sin(theta);
sype 10:ae3178aa94e9 80 y = yO - R*cos(theta);
sype 12:d5e21f71c2a9 81 }*/
IceTeam 36:2d7357a385bc 82
IceTeam 36:2d7357a385bc 83
IceTeam 36:2d7357a385bc 84
IceTeam 36:2d7357a385bc 85 double dx = deltaS*cos(theta+deltaTheta/2);
IceTeam 36:2d7357a385bc 86 double dy = deltaS*sin(theta+deltaTheta/2);
IceTeam 36:2d7357a385bc 87 x += dx;
IceTeam 36:2d7357a385bc 88 y += dy;
IceTeam 36:2d7357a385bc 89
Near32 33:7f8c29ddee61 90 theta += deltaTheta;
Near32 33:7f8c29ddee61 91 while(theta > PI) theta -= 2*PI;
Near32 33:7f8c29ddee61 92 while(theta <= -PI) theta += 2*PI;
sype 0:ad9600df4a70 93 }
sype 0:ad9600df4a70 94
sype 12:d5e21f71c2a9 95 void Odometry::GotoXY(double x_goal, double y_goal)
sype 12:d5e21f71c2a9 96 {
IceTeam 78:c28bdbf29b6e 97 saved_x_goal = x_goal;
IceTeam 78:c28bdbf29b6e 98 saved_y_goal = y_goal;
IceTeam 78:c28bdbf29b6e 99
IceTeam 78:c28bdbf29b6e 100 currentStep = STEP_D;
IceTeam 78:c28bdbf29b6e 101 currentMainFunction = MAIN_FCT_XY;
IceTeam 78:c28bdbf29b6e 102
sype 12:d5e21f71c2a9 103 double theta_ = atan2(y_goal-y, x_goal-x);
sype 12:d5e21f71c2a9 104 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 12:d5e21f71c2a9 105 GotoThet(theta_);
sype 12:d5e21f71c2a9 106 GotoDist(dist_);
sype 12:d5e21f71c2a9 107 }
sype 12:d5e21f71c2a9 108
sype 10:ae3178aa94e9 109 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
sype 10:ae3178aa94e9 110 {
IceTeam 78:c28bdbf29b6e 111 saved_x_goal = x_goal;
IceTeam 78:c28bdbf29b6e 112 saved_y_goal = y_goal;
IceTeam 78:c28bdbf29b6e 113 saved_theta_goal = theta_goal;
IceTeam 78:c28bdbf29b6e 114
IceTeam 78:c28bdbf29b6e 115 currentStep = STEP_D;
IceTeam 78:c28bdbf29b6e 116 currentMainFunction = MAIN_FCT_XYT;
IceTeam 78:c28bdbf29b6e 117
sype 0:ad9600df4a70 118 double theta_ = atan2(y_goal-y, x_goal-x);
sype 3:62e9d715de65 119 double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
sype 2:abdf8c6823a1 120 GotoThet(theta_);
sype 3:62e9d715de65 121 GotoDist(dist_);
sype 12:d5e21f71c2a9 122 GotoThet(theta_goal);
sype 0:ad9600df4a70 123 }
sype 2:abdf8c6823a1 124
sype 10:ae3178aa94e9 125 void Odometry::GotoThet(double theta_)
sype 10:ae3178aa94e9 126 {
IceTeam 78:c28bdbf29b6e 127 saved_theta_goal = theta_;
IceTeam 78:c28bdbf29b6e 128 currentStep = STEP_T;
sype 10:ae3178aa94e9 129 //pos_prog++;
sype 37:da3a2c781672 130 //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
sype 10:ae3178aa94e9 131 //arrived = false;
sype 10:ae3178aa94e9 132 int32_t distance_ticks_left;
sype 10:ae3178aa94e9 133 int32_t distance_ticks_right;
sype 10:ae3178aa94e9 134
sype 10:ae3178aa94e9 135 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
sype 4:3e6e78d6d3d9 136
sype 4:3e6e78d6d3d9 137 // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
sype 3:62e9d715de65 138 double erreur_theta = theta_ - getTheta();
sype 4:3e6e78d6d3d9 139
sype 3:62e9d715de65 140 while(erreur_theta >= PI) erreur_theta -= 2*PI;
sype 12:d5e21f71c2a9 141 while(erreur_theta < -PI) erreur_theta += 2*PI;
sype 12:d5e21f71c2a9 142
sype 12:d5e21f71c2a9 143 if(erreur_theta < 0) {
sype 12:d5e21f71c2a9 144 distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
sype 12:d5e21f71c2a9 145 distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
sype 4:3e6e78d6d3d9 146 } else {
sype 12:d5e21f71c2a9 147 distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
sype 12:d5e21f71c2a9 148 distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
sype 2:abdf8c6823a1 149 }
sype 10:ae3178aa94e9 150
sype 37:da3a2c781672 151 //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 152 //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
sype 37:da3a2c781672 153 //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
sype 10:ae3178aa94e9 154
sype 46:5658af4e5149 155 roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
sype 10:ae3178aa94e9 156
sype 37:da3a2c781672 157 //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 158
IceTeam 78:c28bdbf29b6e 159 //while(abs(m_pulses_right - distance_ticks_right) >= 1 && abs(m_pulses_left - distance_ticks_left) >= 1) wait(0.001);
IceTeam 78:c28bdbf29b6e 160
IceTeam 78:c28bdbf29b6e 161 while(abs(erreur_theta) > 2.f*PI/180.f)
IceTeam 78:c28bdbf29b6e 162 {
IceTeam 78:c28bdbf29b6e 163 wait(0.001);
IceTeam 78:c28bdbf29b6e 164
IceTeam 78:c28bdbf29b6e 165 erreur_theta = theta_ - getTheta();
IceTeam 78:c28bdbf29b6e 166 while(erreur_theta >= PI) erreur_theta -= 2*PI;
IceTeam 78:c28bdbf29b6e 167 while(erreur_theta < -PI) erreur_theta += 2*PI;
sype 77:f19cc7f81f2a 168 }
IceTeam 78:c28bdbf29b6e 169
sype 77:f19cc7f81f2a 170 //logger.printf ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left);
IceTeam 47:be4eebf40568 171 //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 172
IceTeam 31:8bcc3a0bfa8a 173 wait(0.4);
IceTeam 78:c28bdbf29b6e 174
IceTeam 78:c28bdbf29b6e 175 /*roboclaw.ForwardM1(0);
IceTeam 78:c28bdbf29b6e 176 roboclaw.ForwardM2(0);*/
IceTeam 78:c28bdbf29b6e 177
IceTeam 78:c28bdbf29b6e 178 //theta = theta_;
sype 10:ae3178aa94e9 179 //arrived = true;
sype 37:da3a2c781672 180 //logger.printf("arrivey %d\n\r",pos_prog);
sype 2:abdf8c6823a1 181 }
sype 2:abdf8c6823a1 182
sype 10:ae3178aa94e9 183 void Odometry::GotoDist(double distance)
sype 10:ae3178aa94e9 184 {
IceTeam 78:c28bdbf29b6e 185 currentStep = STEP_D;
sype 10:ae3178aa94e9 186 //pos_prog++;
sype 37:da3a2c781672 187 //logger.printf("Dist : %3.2f\n\r", distance);
sype 10:ae3178aa94e9 188 //arrived = false;
sype 10:ae3178aa94e9 189
sype 10:ae3178aa94e9 190 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
sype 10:ae3178aa94e9 191
sype 10:ae3178aa94e9 192 int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
sype 10:ae3178aa94e9 193 int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
sype 77:f19cc7f81f2a 194 if (distance >= 0)
sype 77:f19cc7f81f2a 195 roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
sype 77:f19cc7f81f2a 196 else
sype 77:f19cc7f81f2a 197 roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, -vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, -vitesse_dista, deccel_dista, distance_ticks_left, 1);
sype 37:da3a2c781672 198 //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 199
IceTeam 78:c28bdbf29b6e 200 while(abs(m_pulses_right - distance_ticks_right) >= 30 || abs(m_pulses_left - distance_ticks_left) >= 30) //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 201 {
IceTeam 78:c28bdbf29b6e 202 wait(0.1);
sype 77:f19cc7f81f2a 203 }
IceTeam 78:c28bdbf29b6e 204
IceTeam 78:c28bdbf29b6e 205 /*while( carre(x-saved_x_goal) + carre(y-saved_y_goal) > 4*4)
IceTeam 78:c28bdbf29b6e 206 {
IceTeam 78:c28bdbf29b6e 207 wait(0.1);
IceTeam 78:c28bdbf29b6e 208 logger.printf("Dist error : %f (%f,%f -> %f,%f)\r\n", sqrt(carre(x-saved_x_goal) + carre(y-saved_y_goal)),x,y,saved_x_goal,saved_y_goal);
IceTeam 78:c28bdbf29b6e 209 }*/
IceTeam 78:c28bdbf29b6e 210
IceTeam 31:8bcc3a0bfa8a 211 wait(0.4);
IceTeam 78:c28bdbf29b6e 212
IceTeam 78:c28bdbf29b6e 213 /*roboclaw.ForwardM1(0);
IceTeam 78:c28bdbf29b6e 214 roboclaw.ForwardM2(0);*/
IceTeam 78:c28bdbf29b6e 215
sype 37:da3a2c781672 216 //logger.printf("arrivey %d\n\r",pos_prog);
sype 37:da3a2c781672 217 //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
sype 2:abdf8c6823a1 218 }
IceTeam 35:4e3d9ab1b94b 219
IceTeam 35:4e3d9ab1b94b 220 void Odometry::TestEntraxe(int i) {
IceTeam 35:4e3d9ab1b94b 221 int32_t distance_ticks_left;
IceTeam 35:4e3d9ab1b94b 222 int32_t distance_ticks_right;
IceTeam 35:4e3d9ab1b94b 223
IceTeam 35:4e3d9ab1b94b 224 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
IceTeam 35:4e3d9ab1b94b 225
IceTeam 35:4e3d9ab1b94b 226 double erreur_theta = 2*PI*i - getTheta();
IceTeam 35:4e3d9ab1b94b 227 if(erreur_theta < 0) {
IceTeam 35:4e3d9ab1b94b 228 distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
IceTeam 35:4e3d9ab1b94b 229 distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
IceTeam 35:4e3d9ab1b94b 230 } else {
IceTeam 35:4e3d9ab1b94b 231 distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
IceTeam 35:4e3d9ab1b94b 232 distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
IceTeam 35:4e3d9ab1b94b 233 }
IceTeam 35:4e3d9ab1b94b 234
sype 46:5658af4e5149 235 roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
IceTeam 35:4e3d9ab1b94b 236
IceTeam 78:c28bdbf29b6e 237 while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
IceTeam 47:be4eebf40568 238
IceTeam 35:4e3d9ab1b94b 239 wait(0.4);
IceTeam 35:4e3d9ab1b94b 240 }
IceTeam 39:309f38d1e49c 241
IceTeam 39:309f38d1e49c 242 void Odometry::Forward(float i) {
IceTeam 39:309f38d1e49c 243 int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
IceTeam 39:309f38d1e49c 244
IceTeam 39:309f38d1e49c 245 int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
IceTeam 39:309f38d1e49c 246 int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
IceTeam 39:309f38d1e49c 247
sype 46:5658af4e5149 248 roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
IceTeam 39:309f38d1e49c 249
IceTeam 39:309f38d1e49c 250 //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 251
IceTeam 39:309f38d1e49c 252 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 253 wait(0.4);
IceTeam 78:c28bdbf29b6e 254 }
IceTeam 78:c28bdbf29b6e 255
IceTeam 78:c28bdbf29b6e 256 void Odometry::stop() {
IceTeam 78:c28bdbf29b6e 257 paused = false;
IceTeam 78:c28bdbf29b6e 258 timer.stop();
IceTeam 78:c28bdbf29b6e 259 timer.reset();
IceTeam 78:c28bdbf29b6e 260
IceTeam 78:c28bdbf29b6e 261 roboclaw.ForwardM1(0);
IceTeam 78:c28bdbf29b6e 262 roboclaw.ForwardM2(0);
IceTeam 78:c28bdbf29b6e 263 }
IceTeam 78:c28bdbf29b6e 264
IceTeam 78:c28bdbf29b6e 265 void Odometry::pause() {
IceTeam 78:c28bdbf29b6e 266 timer.start();
IceTeam 78:c28bdbf29b6e 267 timer.reset();
IceTeam 78:c28bdbf29b6e 268 paused = true;
IceTeam 78:c28bdbf29b6e 269 roboclaw.ForwardM1(0);
IceTeam 78:c28bdbf29b6e 270 roboclaw.ForwardM2(0);
IceTeam 78:c28bdbf29b6e 271 }
IceTeam 78:c28bdbf29b6e 272
IceTeam 78:c28bdbf29b6e 273 void Odometry::resume() {
IceTeam 78:c28bdbf29b6e 274 if(paused && timer.read() > 0.5f)
IceTeam 78:c28bdbf29b6e 275 {
IceTeam 78:c28bdbf29b6e 276 paused = false;
IceTeam 78:c28bdbf29b6e 277 timer.stop();
IceTeam 78:c28bdbf29b6e 278 timer.reset();
IceTeam 78:c28bdbf29b6e 279
IceTeam 78:c28bdbf29b6e 280 if(currentMainFunction == MAIN_FCT_XY)
IceTeam 78:c28bdbf29b6e 281 GotoXY(saved_x_goal,saved_y_goal);
IceTeam 78:c28bdbf29b6e 282 else
IceTeam 78:c28bdbf29b6e 283 GotoXYT(saved_x_goal,saved_y_goal,saved_theta_goal);
IceTeam 78:c28bdbf29b6e 284
IceTeam 78:c28bdbf29b6e 285
IceTeam 78:c28bdbf29b6e 286 }
IceTeam 39:309f38d1e49c 287 }