ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@116:73d7d87e0299, 2015-05-06 (annotated)

Committer:
sype
Date:
Wed May 06 11:14:02 2015 +0000
Revision:
116:73d7d87e0299
Parent:
114:be06d518b4a7
Child:
118:09bfc62d4867
A B

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 39:09c04fd42c94 2
Jagang 39:09c04fd42c94 3 #include "defines.h"
Jagang 39:09c04fd42c94 4
Jagang 0:41149573d577 5 #include "QEI.h"
Jagang 23:228317fe0457 6 #include "Map.h"
Jagang 109:53918ba98306 7
Jagang 109:53918ba98306 8 #include "Objectif.h"
Jagang 109:53918ba98306 9 #include "Obj_clap.h"
Jagang 109:53918ba98306 10
Jagang 109:53918ba98306 11
Jagang 39:09c04fd42c94 12 #include "AX12.h"
Jagang 0:41149573d577 13
Jagang 71:95d76c181b22 14 #ifdef PLAN_A
Jagang 71:95d76c181b22 15 #include "Odometry.h"
Jagang 71:95d76c181b22 16 #include "Asserv.h"
Jagang 71:95d76c181b22 17 #else
Jagang 71:95d76c181b22 18 #include "Odometry2.h"
Jagang 71:95d76c181b22 19 #include "planB.h"
Jagang 71:95d76c181b22 20 #endif
Jagang 71:95d76c181b22 21
Near32 36:54f86bc6fd80 22 #include "Motor.h"
Jagang 93:4d5664e9188a 23
Jagang 93:4d5664e9188a 24 void update();
Jagang 93:4d5664e9188a 25
Jagang 93:4d5664e9188a 26 // *--------------------------* //
Jagang 93:4d5664e9188a 27 // Déclarations //
Jagang 93:4d5664e9188a 28
Jagang 93:4d5664e9188a 29 // Decl. logger //
Jagang 93:4d5664e9188a 30
Jagang 93:4d5664e9188a 31 Serial logger(OUT_TX, OUT_RX); // tx, rx
Jagang 93:4d5664e9188a 32
Jagang 93:4d5664e9188a 33 // Decl. timer //
Jagang 93:4d5664e9188a 34
Jagang 93:4d5664e9188a 35 Timer t;
Jagang 93:4d5664e9188a 36 Ticker ticker;
Jagang 93:4d5664e9188a 37
Jagang 93:4d5664e9188a 38 // Decl. AX12 //
Jagang 93:4d5664e9188a 39
Jagang 93:4d5664e9188a 40 AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
Jagang 93:4d5664e9188a 41 AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
Jagang 93:4d5664e9188a 42 AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);
Jagang 93:4d5664e9188a 43
Jagang 93:4d5664e9188a 44 // Decl. Moteurs //
Jagang 93:4d5664e9188a 45
Jagang 93:4d5664e9188a 46 PwmOut pw1(PWM_MOT1);
Jagang 93:4d5664e9188a 47 DigitalOut dir1(DIR_MOT1);
Jagang 93:4d5664e9188a 48 PwmOut pw2(PWM_MOT2);
Jagang 93:4d5664e9188a 49 DigitalOut dir2(DIR_MOT2);
Jagang 93:4d5664e9188a 50
Jagang 93:4d5664e9188a 51 Motor motorL(PWM_MOT1,DIR_MOT1);
Jagang 93:4d5664e9188a 52 Motor motorR(PWM_MOT2,DIR_MOT2);
Jagang 93:4d5664e9188a 53
Jagang 93:4d5664e9188a 54 // Decl. Sharps //
sype 80:5399183aa39b 55
Jagang 93:4d5664e9188a 56 AnalogIn sharp_devant(SHARP_D);
Jagang 93:4d5664e9188a 57 AnalogIn sharp_devant_droite(SHARP_DD);
Jagang 93:4d5664e9188a 58 AnalogIn sharp_devant_gauche(SHARP_DG);
Jagang 93:4d5664e9188a 59 AnalogIn sharp_arriere_gauche(SHARP_AG);
Jagang 93:4d5664e9188a 60 AnalogIn sharp_arriere_droite(SHARP_AD);
Jagang 93:4d5664e9188a 61
Jagang 93:4d5664e9188a 62 // Decl. Boutons //
Jagang 93:4d5664e9188a 63
Jagang 93:4d5664e9188a 64 DigitalIn bp(BP_DESSUS);
Jagang 93:4d5664e9188a 65 DigitalIn tirette(TIRETTE_DESSUS);
Jagang 93:4d5664e9188a 66 DigitalIn couleur(COULEUR_DESSUS);
Jagang 93:4d5664e9188a 67
Jagang 93:4d5664e9188a 68 // Decl. Odometrie //
Jagang 21:5443f93819db 69
Jagang 93:4d5664e9188a 70 #ifdef PLAN_A
Jagang 93:4d5664e9188a 71 QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 72 QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 73
Near32 107:a6e498b5706c 74 Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
Jagang 93:4d5664e9188a 75 #else
Jagang 93:4d5664e9188a 76 QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 77 QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 78
Jagang 103:6a38cc0765f5 79 Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
Jagang 93:4d5664e9188a 80 #endif
Jagang 93:4d5664e9188a 81
Jagang 93:4d5664e9188a 82 // Decl. Asserv //
Jagang 93:4d5664e9188a 83
Jagang 93:4d5664e9188a 84 #ifdef PLAN_A
Jagang 109:53918ba98306 85 Asserv<float> asserv(&odometry);
Jagang 93:4d5664e9188a 86 #else
Jagang 93:4d5664e9188a 87 aserv_planB asserv(odometry,motorL,motorR);
Jagang 93:4d5664e9188a 88 #endif
Jagang 93:4d5664e9188a 89
Jagang 109:53918ba98306 90 // Decl. IA //
Jagang 109:53918ba98306 91
Jagang 109:53918ba98306 92 Map terrain;
Jagang 109:53918ba98306 93 std::vector<Objectif*> objectifs;
Jagang 109:53918ba98306 94 char couleurRobot = COULEUR_JAUNE;
Jagang 109:53918ba98306 95
Jagang 93:4d5664e9188a 96 // Fin Decl. //
Jagang 93:4d5664e9188a 97 // *--------------------------* //
Jagang 93:4d5664e9188a 98
Jagang 23:228317fe0457 99
Jagang 0:41149573d577 100 int main()
Jagang 0:41149573d577 101 {
Jagang 93:4d5664e9188a 102 #ifdef OUT_USB
sype 116:73d7d87e0299 103 logger.baud((int)921600);
Jagang 93:4d5664e9188a 104 #endif
Jagang 74:88be86f83d17 105
Jagang 93:4d5664e9188a 106 // *--------------------------* //
Jagang 93:4d5664e9188a 107 // Init //
Near32 17:f360e21d3307 108
Jagang 93:4d5664e9188a 109 logger.printf("Initialisation.............");
Jagang 57:ab13f4e7a2b2 110
Jagang 93:4d5664e9188a 111 // Init. AX12 //
Jagang 39:09c04fd42c94 112
Jagang 93:4d5664e9188a 113 ax12_pince.setMode(0);
Jagang 93:4d5664e9188a 114 ax12_brasG.setMode(0);
Jagang 93:4d5664e9188a 115 ax12_brasD.setMode(0);
Jagang 54:e0e58c36658a 116
Jagang 93:4d5664e9188a 117 // Init. Moteurs //
Near32 24:3c0422e1ebd6 118
Jagang 93:4d5664e9188a 119 motorL.setSpeed(0);
Jagang 93:4d5664e9188a 120 motorR.setSpeed(0);
Jagang 93:4d5664e9188a 121
Jagang 93:4d5664e9188a 122 // Init. Sharps //
Jagang 93:4d5664e9188a 123
Jagang 93:4d5664e9188a 124 // Init. Boutons //
Jagang 93:4d5664e9188a 125
Jagang 93:4d5664e9188a 126 // Init. Odometrie //
Jagang 93:4d5664e9188a 127
Jagang 71:95d76c181b22 128 #ifdef PLAN_A
Jagang 78:7c7cefbe1772 129 odometry.setTheta(0);
Jagang 78:7c7cefbe1772 130 odometry.setX(0);
Jagang 78:7c7cefbe1772 131 odometry.setY(0);
Jagang 71:95d76c181b22 132 #else
Jagang 71:95d76c181b22 133 odometry.setTheta(PI/2);
Jagang 71:95d76c181b22 134 odometry.setX(0);
Jagang 71:95d76c181b22 135 odometry.setY(0);
Jagang 71:95d76c181b22 136 #endif
Jagang 60:546d7b43333b 137
Jagang 109:53918ba98306 138 // Init. IA //
Jagang 109:53918ba98306 139
Jagang 109:53918ba98306 140 terrain.build();
Jagang 109:53918ba98306 141
Jagang 93:4d5664e9188a 142 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 143
Jagang 93:4d5664e9188a 144 // Fin Init. //
Jagang 93:4d5664e9188a 145 // *--------------------------* //
Jagang 93:4d5664e9188a 146
Jagang 93:4d5664e9188a 147 // *--------------------------* //
Jagang 93:4d5664e9188a 148 // MIP //
Jagang 93:4d5664e9188a 149
Jagang 109:53918ba98306 150 logger.printf("Appuyer sur le bouton pour mettre en position\r\n");
Jagang 93:4d5664e9188a 151
Jagang 93:4d5664e9188a 152 while(!bp); // On attend le top de mise en position
Jagang 93:4d5664e9188a 153
Jagang 93:4d5664e9188a 154 logger.printf("MIP........................");
Jagang 93:4d5664e9188a 155
Jagang 110:7e71e5cd8197 156 odometry.update(1);
Jagang 110:7e71e5cd8197 157
Jagang 110:7e71e5cd8197 158 odometry.setTheta(PI/2);
Jagang 110:7e71e5cd8197 159 odometry.setX(1000);
Jagang 110:7e71e5cd8197 160 odometry.setY(400);
Jagang 110:7e71e5cd8197 161
Jagang 93:4d5664e9188a 162 ax12_pince.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 163 ax12_brasG.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 164 ax12_brasD.setMaxTorque(MAX_TORQUE);
Jagang 39:09c04fd42c94 165
Jagang 93:4d5664e9188a 166 ax12_pince.setGoal(PINCE_FERMEE);
Jagang 93:4d5664e9188a 167 ax12_brasG.setGoal(BRASG_OUVERT);
Jagang 93:4d5664e9188a 168 ax12_brasD.setGoal(BRASD_OUVERT);
Jagang 103:6a38cc0765f5 169 wait(0.5);
Jagang 93:4d5664e9188a 170 ax12_pince.setGoal(PINCE_OUVERTE);
Jagang 93:4d5664e9188a 171 ax12_brasG.setGoal(BRASG_FERME);
Jagang 93:4d5664e9188a 172 ax12_brasD.setGoal(BRASD_FERME);
Jagang 93:4d5664e9188a 173 wait(1.5);
Jagang 93:4d5664e9188a 174 ax12_pince.setMaxTorque(0);
Jagang 93:4d5664e9188a 175 ax12_brasG.setMaxTorque(0);
Jagang 93:4d5664e9188a 176 ax12_brasD.setMaxTorque(0);
Jagang 93:4d5664e9188a 177
Jagang 93:4d5664e9188a 178 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 179
Jagang 93:4d5664e9188a 180 // //
Jagang 93:4d5664e9188a 181 // *--------------------------* //
Jagang 93:4d5664e9188a 182
Jagang 93:4d5664e9188a 183 // *--------------------------* //
Jagang 93:4d5664e9188a 184 // Asserv //
Jagang 93:4d5664e9188a 185
Jagang 93:4d5664e9188a 186 logger.printf("Demarrage asserv...........");
Jagang 93:4d5664e9188a 187 t.start();
Jagang 93:4d5664e9188a 188 ticker.attach_us(&update,10000); //100Hz
Jagang 93:4d5664e9188a 189 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 190
Jagang 93:4d5664e9188a 191 // //
Jagang 93:4d5664e9188a 192 // *--------------------------* //
Jagang 109:53918ba98306 193
Jagang 109:53918ba98306 194 // *--------------------------* //
Jagang 109:53918ba98306 195 // Tirrette + couleur //
Jagang 109:53918ba98306 196
Jagang 109:53918ba98306 197 //while(tirette); // La tirrette
Jagang 109:53918ba98306 198
Jagang 109:53918ba98306 199 if(couleur == COULEUR_JAUNE)
Jagang 109:53918ba98306 200 {
Jagang 109:53918ba98306 201 couleurRobot = COULEUR_JAUNE;
Jagang 109:53918ba98306 202 objectifs.push_back(new Obj_clap(0, 0, 0, &asserv, &ax12_brasG,&ax12_brasD));
Jagang 109:53918ba98306 203 }
Jagang 109:53918ba98306 204 else
Jagang 109:53918ba98306 205 {
Jagang 109:53918ba98306 206 couleurRobot = COULEUR_VERTE;
Jagang 109:53918ba98306 207 }
Jagang 109:53918ba98306 208
Jagang 109:53918ba98306 209
Jagang 109:53918ba98306 210 // //
Jagang 109:53918ba98306 211 // *--------------------------* //
Jagang 93:4d5664e9188a 212
Jagang 93:4d5664e9188a 213 bool continuer = true;
Jagang 71:95d76c181b22 214
Jagang 109:53918ba98306 215 // *--------------------------* //
Jagang 109:53918ba98306 216 // IA //
Jagang 109:53918ba98306 217
Jagang 110:7e71e5cd8197 218 #define IA
Jagang 110:7e71e5cd8197 219 #ifdef IA
Jagang 109:53918ba98306 220
sype 116:73d7d87e0299 221 logger.printf("Pathfinding ... ");
Jagang 110:7e71e5cd8197 222 Timer t;
Jagang 110:7e71e5cd8197 223 t.start();
sype 116:73d7d87e0299 224 int i = terrain.AStar(1000,400,1750,600,10);
Jagang 110:7e71e5cd8197 225 t.stop();
Jagang 110:7e71e5cd8197 226 logger.printf("[done]\r\n");
Jagang 110:7e71e5cd8197 227
Jagang 110:7e71e5cd8197 228 logger.printf("Return : %d in %.3fms\r\n",i,t.read()*1000);
Jagang 110:7e71e5cd8197 229
sype 116:73d7d87e0299 230 for(i=1;i<terrain.path.size()-1;i++)
Jagang 109:53918ba98306 231 {
Jagang 110:7e71e5cd8197 232 logger.printf("%d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
Jagang 110:7e71e5cd8197 233 asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
Jagang 110:7e71e5cd8197 234 while(!asserv.isArrived())wait(0.1);
Jagang 109:53918ba98306 235 }
Jagang 114:be06d518b4a7 236 asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
Jagang 114:be06d518b4a7 237 while(!asserv.isArrived())wait(0.1);
Jagang 110:7e71e5cd8197 238
Jagang 110:7e71e5cd8197 239 logger.printf("Finit !! \r\n");
Jagang 110:7e71e5cd8197 240
sype 116:73d7d87e0299 241 while(1);
Jagang 109:53918ba98306 242
Jagang 109:53918ba98306 243 while(continuer)
Jagang 109:53918ba98306 244 {
Jagang 109:53918ba98306 245 int objAct = 0;
Jagang 109:53918ba98306 246 bool newObj = false;
Jagang 109:53918ba98306 247 for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
Jagang 109:53918ba98306 248 {
Jagang 109:53918ba98306 249 if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
Jagang 109:53918ba98306 250 continue;
Jagang 109:53918ba98306 251
Jagang 109:53918ba98306 252 if(objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
Jagang 109:53918ba98306 253 continue;
Jagang 109:53918ba98306 254
Jagang 109:53918ba98306 255 if(terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 0.05) != 1) // L'objectif est atteignable !!
Jagang 109:53918ba98306 256 {
Jagang 109:53918ba98306 257 newObj = true;
Jagang 109:53918ba98306 258 break;
Jagang 109:53918ba98306 259 }
Jagang 109:53918ba98306 260 }
Jagang 109:53918ba98306 261
Jagang 109:53918ba98306 262 if(newObj)
Jagang 109:53918ba98306 263 {
Jagang 114:be06d518b4a7 264 /* Déplacement vers le nouvel objectif */
Jagang 114:be06d518b4a7 265 for(i=0;i<terrain.path.size()-1;i++)
Jagang 114:be06d518b4a7 266 {
Jagang 114:be06d518b4a7 267 logger.printf("%d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
Jagang 114:be06d518b4a7 268 asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
Jagang 114:be06d518b4a7 269 while(!asserv.isArrived())wait(0.1);
Jagang 114:be06d518b4a7 270 }
Jagang 114:be06d518b4a7 271 asserv.setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
Jagang 114:be06d518b4a7 272 while(!asserv.isArrived())wait(0.1);
Jagang 109:53918ba98306 273
Jagang 114:be06d518b4a7 274 objectifs[objAct]->run();
Jagang 109:53918ba98306 275 }
Jagang 109:53918ba98306 276 else
Jagang 109:53918ba98306 277 {
Jagang 109:53918ba98306 278 logger.printf("Nothind to be done ... :( \r\n");
Jagang 109:53918ba98306 279 wait(1);
Jagang 109:53918ba98306 280 }
Jagang 109:53918ba98306 281 }
Jagang 109:53918ba98306 282
Jagang 114:be06d518b4a7 283 #endif
Jagang 114:be06d518b4a7 284
Jagang 71:95d76c181b22 285 #ifdef PLAN_A
Jagang 109:53918ba98306 286 asserv.setGoal(300.0f,200.0f,0.0f);
Near32 89:d05001d85a02 287 logger.printf("GOAL SET... RUNNING!\r\n");
Jagang 71:95d76c181b22 288
Jagang 74:88be86f83d17 289 char state = 0;
Jagang 74:88be86f83d17 290
Jagang 93:4d5664e9188a 291 while(continuer)
Jagang 71:95d76c181b22 292 {
Near32 107:a6e498b5706c 293 //#define test
Near32 107:a6e498b5706c 294 #ifndef test
Jagang 111:c8a1129691da 295 if(state == 0 && asserv.isArrived())
Near32 107:a6e498b5706c 296 {
Near32 107:a6e498b5706c 297 state = 1;
Near32 107:a6e498b5706c 298 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 107:a6e498b5706c 299 motorR.setSpeed(0.0f);
Near32 107:a6e498b5706c 300 motorL.setSpeed(0.0f);
Near32 107:a6e498b5706c 301 wait(5);
Jagang 111:c8a1129691da 302 asserv.setGoal(0.0f,0.0f,0.0f);
Near32 107:a6e498b5706c 303 }
Jagang 111:c8a1129691da 304 else if(state == 1 && asserv.isArrived())
Near32 107:a6e498b5706c 305 {
Near32 107:a6e498b5706c 306 state = 0;
Near32 107:a6e498b5706c 307 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 107:a6e498b5706c 308 motorR.setSpeed(0.0f);
Near32 107:a6e498b5706c 309 motorL.setSpeed(0.0f);
Near32 107:a6e498b5706c 310 wait(5);
Jagang 111:c8a1129691da 311 asserv.setGoal(0.0f,0.0f,0.0f);
Near32 107:a6e498b5706c 312 }
Near32 107:a6e498b5706c 313 #endif
Jagang 92:b403f2724252 314 #ifdef test
Jagang 109:53918ba98306 315 if(state == 0 && asserv.isArrivedRho())
Jagang 92:b403f2724252 316 {
Jagang 92:b403f2724252 317 state = 1;
Jagang 92:b403f2724252 318 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 98:2ec4e17dfc92 319 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 320 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 321 wait(5);
Jagang 109:53918ba98306 322 asserv.setGoal(300.0f,200.0f,0.0f);
Jagang 92:b403f2724252 323 }
Jagang 109:53918ba98306 324 else if(state == 1 && asserv.isArrivedRho())
Jagang 92:b403f2724252 325 {
Jagang 92:b403f2724252 326 state = 2;
Jagang 92:b403f2724252 327 logger.printf("Arrive en 200,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 328 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 329 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 330 wait(5);
Jagang 109:53918ba98306 331 asserv.setGoal(0.0f,300.0f,0.0f);
Jagang 92:b403f2724252 332 }
Jagang 109:53918ba98306 333 else if(state == 2 && asserv.isArrivedRho())
Jagang 92:b403f2724252 334 {
Jagang 92:b403f2724252 335 state = 0;
Jagang 92:b403f2724252 336 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 337 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 338 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 339 wait(5);
Jagang 109:53918ba98306 340 asserv.setGoal(0.0f,0.0f,0.0f);
Jagang 92:b403f2724252 341 }
Jagang 92:b403f2724252 342 #endif
Jagang 71:95d76c181b22 343 }
Jagang 71:95d76c181b22 344 #else
sype 108:890094ee202a 345 asserv.setGoal(1,300,0);
sype 108:890094ee202a 346 logger.printf("Attente\r\n");
sype 108:890094ee202a 347 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 348 logger.printf("Arrivé\r\n");
sype 108:890094ee202a 349 wait(1);
sype 108:890094ee202a 350 asserv.setGoal(-300,300,0);
sype 108:890094ee202a 351 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 352 wait(1);
sype 108:890094ee202a 353 asserv.setGoal(-300,-1,0);
sype 108:890094ee202a 354 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 355 wait(1);
sype 108:890094ee202a 356 asserv.setGoal(-300,-300,0);
sype 108:890094ee202a 357 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 358 wait(1);
sype 108:890094ee202a 359 asserv.setGoal(1,-300,0);
sype 108:890094ee202a 360 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 361 wait(1);
sype 108:890094ee202a 362 asserv.setGoal(300,-300,0);
sype 108:890094ee202a 363 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 364 wait(1);
sype 108:890094ee202a 365 asserv.setGoal(0,0,0);
Jagang 71:95d76c181b22 366 #endif
Jagang 103:6a38cc0765f5 367
Jagang 103:6a38cc0765f5 368 while(1);
Near32 43:87bdce65341f 369 }
Jagang 93:4d5664e9188a 370
Jagang 93:4d5664e9188a 371 void update()
Jagang 93:4d5664e9188a 372 {
Jagang 93:4d5664e9188a 373 float dt = t.read();
Jagang 93:4d5664e9188a 374 t.reset();
Jagang 93:4d5664e9188a 375
Jagang 93:4d5664e9188a 376 odometry.update(dt);
Jagang 109:53918ba98306 377 asserv.update(dt);
Jagang 93:4d5664e9188a 378
Jagang 93:4d5664e9188a 379 #ifdef PLAN_A
Jagang 93:4d5664e9188a 380 float phi_r = (float)instanceAsserv.getPhiR();
Jagang 93:4d5664e9188a 381 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 93:4d5664e9188a 382 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 93:4d5664e9188a 383
Jagang 93:4d5664e9188a 384 motorR.setSpeed(0.08+(phi_r/phi_max));
Jagang 93:4d5664e9188a 385 motorL.setSpeed(0.08+(phi_l/phi_max));
Jagang 93:4d5664e9188a 386 #endif
Jagang 93:4d5664e9188a 387 }
Jagang 93:4d5664e9188a 388