ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@123:55e5e9acc541, 2015-05-11 (annotated)

Committer:
Jagang
Date:
Mon May 11 20:32:11 2015 +0000
Revision:
123:55e5e9acc541
Parent:
122:3d059ad76dd7 
Maj AI; Repositionnement du d?part (1000,177); Ajout de l'action sur les claps

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 117:f8c147141a0c 10 #include "Obj_pince.h"
Jagang 117:f8c147141a0c 11 #include "Obj_depot.h"
Jagang 109:53918ba98306 12
Jagang 109:53918ba98306 13
Jagang 39:09c04fd42c94 14 #include "AX12.h"
Jagang 0:41149573d577 15
Jagang 71:95d76c181b22 16 #ifdef PLAN_A
Jagang 71:95d76c181b22 17 #include "Odometry.h"
Jagang 71:95d76c181b22 18 #include "Asserv.h"
Jagang 71:95d76c181b22 19 #else
Jagang 71:95d76c181b22 20 #include "Odometry2.h"
Jagang 71:95d76c181b22 21 #include "planB.h"
Jagang 71:95d76c181b22 22 #endif
Jagang 71:95d76c181b22 23
Near32 36:54f86bc6fd80 24 #include "Motor.h"
Jagang 93:4d5664e9188a 25
Jagang 93:4d5664e9188a 26 void update();
Jagang 93:4d5664e9188a 27
Jagang 93:4d5664e9188a 28 // *--------------------------* //
Jagang 93:4d5664e9188a 29 // Déclarations //
Jagang 93:4d5664e9188a 30
Jagang 93:4d5664e9188a 31 // Decl. logger //
Jagang 93:4d5664e9188a 32
Jagang 93:4d5664e9188a 33 Serial logger(OUT_TX, OUT_RX); // tx, rx
Jagang 93:4d5664e9188a 34
Jagang 93:4d5664e9188a 35 // Decl. timer //
Jagang 93:4d5664e9188a 36
Jagang 93:4d5664e9188a 37 Timer t;
Jagang 93:4d5664e9188a 38 Ticker ticker;
Jagang 93:4d5664e9188a 39
Jagang 93:4d5664e9188a 40 // Decl. AX12 //
Jagang 93:4d5664e9188a 41
Jagang 93:4d5664e9188a 42 AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
Jagang 93:4d5664e9188a 43 AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
Jagang 93:4d5664e9188a 44 AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);
Jagang 93:4d5664e9188a 45
Jagang 93:4d5664e9188a 46 // Decl. Moteurs //
Jagang 93:4d5664e9188a 47
Jagang 93:4d5664e9188a 48 PwmOut pw1(PWM_MOT1);
Jagang 93:4d5664e9188a 49 DigitalOut dir1(DIR_MOT1);
Jagang 93:4d5664e9188a 50 PwmOut pw2(PWM_MOT2);
Jagang 93:4d5664e9188a 51 DigitalOut dir2(DIR_MOT2);
Jagang 93:4d5664e9188a 52
Jagang 93:4d5664e9188a 53 Motor motorL(PWM_MOT1,DIR_MOT1);
Jagang 93:4d5664e9188a 54 Motor motorR(PWM_MOT2,DIR_MOT2);
Jagang 93:4d5664e9188a 55
Jagang 93:4d5664e9188a 56 // Decl. Sharps //
sype 80:5399183aa39b 57
Jagang 93:4d5664e9188a 58 AnalogIn sharp_devant(SHARP_D);
Jagang 93:4d5664e9188a 59 AnalogIn sharp_devant_droite(SHARP_DD);
Jagang 93:4d5664e9188a 60 AnalogIn sharp_devant_gauche(SHARP_DG);
Jagang 93:4d5664e9188a 61 AnalogIn sharp_arriere_gauche(SHARP_AG);
Jagang 93:4d5664e9188a 62 AnalogIn sharp_arriere_droite(SHARP_AD);
Jagang 93:4d5664e9188a 63
Jagang 93:4d5664e9188a 64 // Decl. Boutons //
Jagang 93:4d5664e9188a 65
Jagang 93:4d5664e9188a 66 DigitalIn bp(BP_DESSUS);
Jagang 93:4d5664e9188a 67 DigitalIn tirette(TIRETTE_DESSUS);
Jagang 93:4d5664e9188a 68 DigitalIn couleur(COULEUR_DESSUS);
Jagang 93:4d5664e9188a 69
Jagang 93:4d5664e9188a 70 // Decl. Odometrie //
Jagang 21:5443f93819db 71
Jagang 93:4d5664e9188a 72 #ifdef PLAN_A
Jagang 93:4d5664e9188a 73 QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 74 QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 75
Near32 107:a6e498b5706c 76 Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
Jagang 93:4d5664e9188a 77 #else
Jagang 93:4d5664e9188a 78 QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 79 QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
Jagang 93:4d5664e9188a 80
Jagang 103:6a38cc0765f5 81 Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
Jagang 93:4d5664e9188a 82 #endif
Jagang 93:4d5664e9188a 83
Jagang 93:4d5664e9188a 84 // Decl. Asserv //
Jagang 93:4d5664e9188a 85
Jagang 93:4d5664e9188a 86 #ifdef PLAN_A
Jagang 109:53918ba98306 87 Asserv<float> asserv(&odometry);
Jagang 93:4d5664e9188a 88 #else
Jagang 93:4d5664e9188a 89 aserv_planB asserv(odometry,motorL,motorR);
Jagang 93:4d5664e9188a 90 #endif
Jagang 93:4d5664e9188a 91
Jagang 109:53918ba98306 92 // Decl. IA //
Jagang 109:53918ba98306 93
Jagang 109:53918ba98306 94 Map terrain;
Jagang 109:53918ba98306 95 std::vector<Objectif*> objectifs;
Jagang 109:53918ba98306 96 char couleurRobot = COULEUR_JAUNE;
Jagang 109:53918ba98306 97
Jagang 93:4d5664e9188a 98 // Fin Decl. //
Jagang 93:4d5664e9188a 99 // *--------------------------* //
Jagang 93:4d5664e9188a 100
Jagang 23:228317fe0457 101
Jagang 0:41149573d577 102 int main()
Jagang 0:41149573d577 103 {
Jagang 93:4d5664e9188a 104 #ifdef OUT_USB
Jagang 117:f8c147141a0c 105 logger.baud((int)921600);
Jagang 93:4d5664e9188a 106 #endif
Jagang 74:88be86f83d17 107
Jagang 93:4d5664e9188a 108 // *--------------------------* //
Jagang 93:4d5664e9188a 109 // Init //
Near32 17:f360e21d3307 110
Jagang 93:4d5664e9188a 111 logger.printf("Initialisation.............");
Jagang 57:ab13f4e7a2b2 112
Jagang 93:4d5664e9188a 113 // Init. AX12 //
Jagang 39:09c04fd42c94 114
Jagang 93:4d5664e9188a 115 ax12_pince.setMode(0);
Jagang 93:4d5664e9188a 116 ax12_brasG.setMode(0);
Jagang 93:4d5664e9188a 117 ax12_brasD.setMode(0);
Jagang 54:e0e58c36658a 118
Jagang 93:4d5664e9188a 119 // Init. Moteurs //
Near32 24:3c0422e1ebd6 120
Jagang 93:4d5664e9188a 121 motorL.setSpeed(0);
Jagang 93:4d5664e9188a 122 motorR.setSpeed(0);
Jagang 93:4d5664e9188a 123
Jagang 93:4d5664e9188a 124 // Init. Sharps //
Jagang 93:4d5664e9188a 125
Jagang 93:4d5664e9188a 126 // Init. Boutons //
Jagang 93:4d5664e9188a 127
Jagang 93:4d5664e9188a 128 // Init. Odometrie //
Jagang 93:4d5664e9188a 129
Jagang 71:95d76c181b22 130 #ifdef PLAN_A
Jagang 78:7c7cefbe1772 131 odometry.setTheta(0);
Jagang 78:7c7cefbe1772 132 odometry.setX(0);
Jagang 78:7c7cefbe1772 133 odometry.setY(0);
Jagang 71:95d76c181b22 134 #else
Jagang 71:95d76c181b22 135 odometry.setTheta(PI/2);
Jagang 71:95d76c181b22 136 odometry.setX(0);
Jagang 71:95d76c181b22 137 odometry.setY(0);
Jagang 71:95d76c181b22 138 #endif
Jagang 60:546d7b43333b 139
Jagang 109:53918ba98306 140 // Init. IA //
Jagang 109:53918ba98306 141
Jagang 109:53918ba98306 142 terrain.build();
Jagang 109:53918ba98306 143
Jagang 93:4d5664e9188a 144 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 145
Jagang 93:4d5664e9188a 146 // Fin Init. //
Jagang 93:4d5664e9188a 147 // *--------------------------* //
Jagang 93:4d5664e9188a 148
Jagang 93:4d5664e9188a 149 // *--------------------------* //
Jagang 93:4d5664e9188a 150 // MIP //
Jagang 93:4d5664e9188a 151
Jagang 109:53918ba98306 152 logger.printf("Appuyer sur le bouton pour mettre en position\r\n");
Jagang 93:4d5664e9188a 153
Jagang 93:4d5664e9188a 154 while(!bp); // On attend le top de mise en position
Jagang 93:4d5664e9188a 155
Jagang 93:4d5664e9188a 156 logger.printf("MIP........................");
Jagang 93:4d5664e9188a 157
Jagang 110:7e71e5cd8197 158 odometry.update(1);
Jagang 110:7e71e5cd8197 159
sype 121:0cc17ba879cb 160 /*odometry.setTheta(PI/2);
Jagang 110:7e71e5cd8197 161 odometry.setX(1000);
sype 121:0cc17ba879cb 162 odometry.setY(400);*/
Jagang 110:7e71e5cd8197 163
Jagang 93:4d5664e9188a 164 ax12_pince.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 165 ax12_brasG.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 166 ax12_brasD.setMaxTorque(MAX_TORQUE);
Jagang 39:09c04fd42c94 167
Jagang 93:4d5664e9188a 168 ax12_pince.setGoal(PINCE_FERMEE);
Jagang 93:4d5664e9188a 169 ax12_brasG.setGoal(BRASG_OUVERT);
Jagang 93:4d5664e9188a 170 ax12_brasD.setGoal(BRASD_OUVERT);
Jagang 103:6a38cc0765f5 171 wait(0.5);
Jagang 93:4d5664e9188a 172 ax12_pince.setGoal(PINCE_OUVERTE);
Jagang 93:4d5664e9188a 173 ax12_brasG.setGoal(BRASG_FERME);
Jagang 93:4d5664e9188a 174 ax12_brasD.setGoal(BRASD_FERME);
Jagang 93:4d5664e9188a 175 wait(1.5);
Jagang 93:4d5664e9188a 176 ax12_pince.setMaxTorque(0);
Jagang 93:4d5664e9188a 177 ax12_brasG.setMaxTorque(0);
Jagang 93:4d5664e9188a 178 ax12_brasD.setMaxTorque(0);
Jagang 93:4d5664e9188a 179
Jagang 93:4d5664e9188a 180 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 181
Jagang 93:4d5664e9188a 182 // //
Jagang 93:4d5664e9188a 183 // *--------------------------* //
Jagang 93:4d5664e9188a 184
Jagang 93:4d5664e9188a 185 // *--------------------------* //
Jagang 93:4d5664e9188a 186 // Asserv //
Jagang 93:4d5664e9188a 187
Jagang 93:4d5664e9188a 188 logger.printf("Demarrage asserv...........");
Jagang 93:4d5664e9188a 189 t.start();
Jagang 93:4d5664e9188a 190 ticker.attach_us(&update,10000); //100Hz
Jagang 93:4d5664e9188a 191 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 192
Jagang 93:4d5664e9188a 193 // //
Jagang 93:4d5664e9188a 194 // *--------------------------* //
Jagang 109:53918ba98306 195
Jagang 109:53918ba98306 196 // *--------------------------* //
Jagang 109:53918ba98306 197 // Tirrette + couleur //
Jagang 109:53918ba98306 198
Jagang 123:55e5e9acc541 199 while(tirette); // La tirrette
Jagang 109:53918ba98306 200
Jagang 109:53918ba98306 201 if(couleur == COULEUR_JAUNE)
Jagang 109:53918ba98306 202 {
Jagang 109:53918ba98306 203 couleurRobot = COULEUR_JAUNE;
Jagang 117:f8c147141a0c 204
Jagang 117:f8c147141a0c 205 odometry.setTheta(PI/2);
Jagang 117:f8c147141a0c 206 odometry.setX(1000);
Jagang 123:55e5e9acc541 207 odometry.setY(177);
Jagang 123:55e5e9acc541 208
Jagang 123:55e5e9acc541 209 objectifs.push_back( new Obj_depot(1000, 350, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 210 objectifs.back()->setId(IDO_DEPOT_PC);
Jagang 117:f8c147141a0c 211
Jagang 123:55e5e9acc541 212 objectifs.push_back( new Obj_depot(1000, 350+100, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 213 objectifs.back()->setId(IDO_DEPOT_P);
Jagang 123:55e5e9acc541 214 objectifs.push_back( new Obj_depot(1000, 350+2*100, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 215 objectifs.back()->setId(IDO_DEPOT_P);
Jagang 123:55e5e9acc541 216 objectifs.push_back( new Obj_depot(1000, 350+3*100, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 217 objectifs.back()->setId(IDO_DEPOT_P);
Jagang 123:55e5e9acc541 218 objectifs.push_back( new Obj_depot(1100, 330+3*100, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 219 objectifs.back()->setId(IDO_DEPOT_P);
Jagang 123:55e5e9acc541 220 objectifs.push_back( new Obj_depot(900, 330+3*100, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 221 objectifs.back()->setId(IDO_DEPOT_P);
Jagang 117:f8c147141a0c 222
Jagang 123:55e5e9acc541 223 //objectifs.push_back( new Obj_pince(800, 620, 800, 700, PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 224 //objectifs.back()->setId(IDO_PC1);
Jagang 123:55e5e9acc541 225 objectifs.push_back( new Obj_pince(1750, 250+350, 1750, 250+190, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 226 objectifs.back()->setId(IDO_PC2);
Jagang 123:55e5e9acc541 227 objectifs.push_back( new Obj_pince(1750, 90+350, 1750, 90+190, -PI/2, &ax12_pince) );
Jagang 123:55e5e9acc541 228 objectifs.back()->setId(IDO_P5);
Jagang 123:55e5e9acc541 229 objectifs.push_back( new Obj_pince(1850-247, 90+247, 1850-130, 90+130, -PI/4, &ax12_pince) );
Jagang 123:55e5e9acc541 230 objectifs.back()->setId(IDO_P6);
Jagang 119:c45efcd706d9 231
Jagang 123:55e5e9acc541 232
Jagang 123:55e5e9acc541 233 objectifs.push_back( new Obj_clap(1750, 192, -PI/4, 1750, 400, PI/2, &ax12_brasG, &ax12_brasD) );
Jagang 119:c45efcd706d9 234
Jagang 123:55e5e9acc541 235 objectifs.push_back( new Obj_pince(1770-247, 1100-247, 1770-135, 1100-135, PI/4, &ax12_pince) );
Jagang 123:55e5e9acc541 236 objectifs.back()->setId(IDO_P7);
Jagang 123:55e5e9acc541 237 objectifs.push_back( new Obj_pince(1355-247, 870-247, 1355-135, 870-135, PI/4, &ax12_pince) );
Jagang 123:55e5e9acc541 238 objectifs.back()->setId(IDO_P4);
Jagang 109:53918ba98306 239 }
Jagang 109:53918ba98306 240 else
Jagang 109:53918ba98306 241 {
Jagang 109:53918ba98306 242 couleurRobot = COULEUR_VERTE;
Jagang 109:53918ba98306 243 }
Jagang 109:53918ba98306 244
Jagang 109:53918ba98306 245
Jagang 109:53918ba98306 246 // //
Jagang 109:53918ba98306 247 // *--------------------------* //
Jagang 93:4d5664e9188a 248
Jagang 93:4d5664e9188a 249 bool continuer = true;
Jagang 71:95d76c181b22 250
Jagang 109:53918ba98306 251 // *--------------------------* //
Jagang 109:53918ba98306 252 // IA //
Jagang 109:53918ba98306 253
Jagang 110:7e71e5cd8197 254 #define IA
Jagang 110:7e71e5cd8197 255 #ifdef IA
Jagang 109:53918ba98306 256
Jagang 109:53918ba98306 257 while(continuer)
Jagang 109:53918ba98306 258 {
Jagang 123:55e5e9acc541 259 if(terrain.getHeight(odometry.getX(), odometry.getY()) >= 32000)
Jagang 123:55e5e9acc541 260 {
Jagang 123:55e5e9acc541 261 logger.printf("Collision, echappement.....");
Jagang 123:55e5e9acc541 262
Jagang 123:55e5e9acc541 263 // Réduction de la taille des obstacles
Jagang 123:55e5e9acc541 264 for(unsigned int i=0;i < terrain.obstacles.size();i++)
Jagang 123:55e5e9acc541 265 while(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
Jagang 123:55e5e9acc541 266 terrain.obstacles[i]->setRobotRadius(terrain.obstacles[i]->getRobotRadius()-1);
Jagang 123:55e5e9acc541 267
Jagang 123:55e5e9acc541 268 logger.printf("[done]\r\n");
Jagang 123:55e5e9acc541 269 }
Jagang 123:55e5e9acc541 270
Jagang 117:f8c147141a0c 271 logger.printf("Recherche d'objectif.......");
Jagang 109:53918ba98306 272 int objAct = 0;
Jagang 109:53918ba98306 273 bool newObj = false;
Jagang 109:53918ba98306 274 for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
Jagang 109:53918ba98306 275 {
Jagang 109:53918ba98306 276 if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
Jagang 109:53918ba98306 277 continue;
Jagang 117:f8c147141a0c 278 if(!objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
Jagang 109:53918ba98306 279 continue;
Jagang 119:c45efcd706d9 280 int retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 10);
Jagang 119:c45efcd706d9 281 if(retourAStar == 1) // L'objectif est atteignable !!
Jagang 119:c45efcd706d9 282 {
Jagang 119:c45efcd706d9 283 newObj = true;
Jagang 119:c45efcd706d9 284 break;
Jagang 119:c45efcd706d9 285 }
Jagang 119:c45efcd706d9 286
Jagang 119:c45efcd706d9 287 if(retourAStar == 3) // Overload memoire
Jagang 119:c45efcd706d9 288 retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 25);
Jagang 119:c45efcd706d9 289
Jagang 117:f8c147141a0c 290 if(retourAStar == 1) // L'objectif est atteignable !!
Jagang 117:f8c147141a0c 291 {
Jagang 117:f8c147141a0c 292 newObj = true;
Jagang 117:f8c147141a0c 293 break;
Jagang 117:f8c147141a0c 294 }
Jagang 109:53918ba98306 295
Jagang 117:f8c147141a0c 296 if(retourAStar == 3) // Overload memoire
Jagang 119:c45efcd706d9 297 retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 50);
Jagang 117:f8c147141a0c 298
Jagang 117:f8c147141a0c 299 if(retourAStar == 1) // L'objectif est atteignable !!
Jagang 109:53918ba98306 300 {
Jagang 109:53918ba98306 301 newObj = true;
Jagang 109:53918ba98306 302 break;
Jagang 109:53918ba98306 303 }
Jagang 109:53918ba98306 304 }
Jagang 109:53918ba98306 305
Jagang 117:f8c147141a0c 306 logger.printf("[done]\r\n");
Jagang 117:f8c147141a0c 307
Jagang 123:55e5e9acc541 308 // On remet la taille des obstacles
Jagang 123:55e5e9acc541 309 for(unsigned int i=0;i < terrain.obstacles.size();i++)
Jagang 123:55e5e9acc541 310 terrain.obstacles[i]->setRobotRadius(ROBOTRADIUS);
Jagang 123:55e5e9acc541 311
Jagang 109:53918ba98306 312 if(newObj)
Jagang 109:53918ba98306 313 {
Jagang 120:06643cbec98a 314 logger.printf("-> Objectif : %d\r\n",objectifs[objAct]->getId());
Jagang 117:f8c147141a0c 315
Jagang 117:f8c147141a0c 316 // Déplacement vers le nouvel objectif //
Jagang 120:06643cbec98a 317
Jagang 117:f8c147141a0c 318 for(int i=1;i<terrain.path.size()-1;i++)
Jagang 114:be06d518b4a7 319 {
Jagang 123:55e5e9acc541 320 logger.printf("Prochain point : %d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
Jagang 114:be06d518b4a7 321 asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
Jagang 114:be06d518b4a7 322 while(!asserv.isArrived())wait(0.1);
Jagang 114:be06d518b4a7 323 }
Jagang 123:55e5e9acc541 324 logger.printf("Dernier point : %.3f\t%.3f\r\n",objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta()*180/PI);
Jagang 114:be06d518b4a7 325 asserv.setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
Jagang 114:be06d518b4a7 326 while(!asserv.isArrived())wait(0.1);
Jagang 109:53918ba98306 327
Jagang 120:06643cbec98a 328
Jagang 120:06643cbec98a 329 // Execution de l'objectif //
Jagang 120:06643cbec98a 330
Jagang 117:f8c147141a0c 331 logger.printf("Execution de l'objectif....");
Jagang 120:06643cbec98a 332
Jagang 114:be06d518b4a7 333 objectifs[objAct]->run();
Jagang 117:f8c147141a0c 334 logger.printf("[done]\r\n");
Jagang 109:53918ba98306 335 }
Jagang 109:53918ba98306 336 else
Jagang 109:53918ba98306 337 {
Jagang 123:55e5e9acc541 338 logger.printf("-> Nothind to be done ... :D \r\n");
Jagang 123:55e5e9acc541 339
Jagang 123:55e5e9acc541 340 ax12_brasG.setMaxTorque(MAX_TORQUE);
Jagang 123:55e5e9acc541 341 ax12_brasD.setMaxTorque(MAX_TORQUE);
Jagang 123:55e5e9acc541 342
Jagang 123:55e5e9acc541 343 ax12_brasG.setGoal(130);
Jagang 123:55e5e9acc541 344 ax12_brasD.setGoal(BRASD_FERME);
Jagang 109:53918ba98306 345 wait(1);
Jagang 123:55e5e9acc541 346 ax12_brasG.setGoal(BRASG_FERME);
Jagang 123:55e5e9acc541 347 ax12_brasD.setGoal(215);
Jagang 123:55e5e9acc541 348 wait(1);
Jagang 123:55e5e9acc541 349 ax12_brasD.setGoal(BRASD_FERME);
Jagang 123:55e5e9acc541 350 wait(1);
Jagang 123:55e5e9acc541 351
Jagang 123:55e5e9acc541 352 ax12_brasG.setMaxTorque(0);
Jagang 123:55e5e9acc541 353 ax12_brasD.setMaxTorque(0);
Jagang 109:53918ba98306 354 }
Jagang 109:53918ba98306 355 }
Jagang 109:53918ba98306 356
Jagang 114:be06d518b4a7 357 #endif
Jagang 114:be06d518b4a7 358
Jagang 71:95d76c181b22 359 #ifdef PLAN_A
Jagang 109:53918ba98306 360 asserv.setGoal(300.0f,200.0f,0.0f);
Near32 89:d05001d85a02 361 logger.printf("GOAL SET... RUNNING!\r\n");
Jagang 71:95d76c181b22 362
Jagang 74:88be86f83d17 363 char state = 0;
Jagang 74:88be86f83d17 364
Jagang 93:4d5664e9188a 365 while(continuer)
Jagang 71:95d76c181b22 366 {
Near32 107:a6e498b5706c 367 //#define test
Near32 107:a6e498b5706c 368 #ifndef test
Jagang 111:c8a1129691da 369 if(state == 0 && asserv.isArrived())
Near32 107:a6e498b5706c 370 {
Near32 107:a6e498b5706c 371 state = 1;
Near32 107:a6e498b5706c 372 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 107:a6e498b5706c 373 motorR.setSpeed(0.0f);
Near32 107:a6e498b5706c 374 motorL.setSpeed(0.0f);
Near32 107:a6e498b5706c 375 wait(5);
Jagang 111:c8a1129691da 376 asserv.setGoal(0.0f,0.0f,0.0f);
Near32 107:a6e498b5706c 377 }
Jagang 111:c8a1129691da 378 else if(state == 1 && asserv.isArrived())
Near32 107:a6e498b5706c 379 {
Near32 107:a6e498b5706c 380 state = 0;
Near32 107:a6e498b5706c 381 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 107:a6e498b5706c 382 motorR.setSpeed(0.0f);
Near32 107:a6e498b5706c 383 motorL.setSpeed(0.0f);
Near32 107:a6e498b5706c 384 wait(5);
Jagang 111:c8a1129691da 385 asserv.setGoal(0.0f,0.0f,0.0f);
Near32 107:a6e498b5706c 386 }
Near32 107:a6e498b5706c 387 #endif
Jagang 92:b403f2724252 388 #ifdef test
Jagang 109:53918ba98306 389 if(state == 0 && asserv.isArrivedRho())
Jagang 92:b403f2724252 390 {
Jagang 92:b403f2724252 391 state = 1;
Jagang 92:b403f2724252 392 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 98:2ec4e17dfc92 393 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 394 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 395 wait(5);
Jagang 109:53918ba98306 396 asserv.setGoal(300.0f,200.0f,0.0f);
Jagang 92:b403f2724252 397 }
Jagang 109:53918ba98306 398 else if(state == 1 && asserv.isArrivedRho())
Jagang 92:b403f2724252 399 {
Jagang 92:b403f2724252 400 state = 2;
Jagang 92:b403f2724252 401 logger.printf("Arrive en 200,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 402 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 403 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 404 wait(5);
Jagang 109:53918ba98306 405 asserv.setGoal(0.0f,300.0f,0.0f);
Jagang 92:b403f2724252 406 }
Jagang 109:53918ba98306 407 else if(state == 2 && asserv.isArrivedRho())
Jagang 92:b403f2724252 408 {
Jagang 92:b403f2724252 409 state = 0;
Jagang 92:b403f2724252 410 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 411 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 412 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 413 wait(5);
Jagang 109:53918ba98306 414 asserv.setGoal(0.0f,0.0f,0.0f);
Jagang 92:b403f2724252 415 }
Jagang 92:b403f2724252 416 #endif
Jagang 71:95d76c181b22 417 }
Jagang 71:95d76c181b22 418 #else
sype 121:0cc17ba879cb 419 asserv.setGoal(1,5);
sype 121:0cc17ba879cb 420 logger.printf("Attente\r\n");
sype 121:0cc17ba879cb 421 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 422 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 423 asserv.setGoal(1,10);
sype 121:0cc17ba879cb 424 logger.printf("Attente\r\n");
sype 121:0cc17ba879cb 425 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 426 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 427 asserv.setGoal(1,15);
sype 108:890094ee202a 428 logger.printf("Attente\r\n");
sype 108:890094ee202a 429 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 430 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 431 asserv.setGoal(1,20);
sype 121:0cc17ba879cb 432 logger.printf("Attente\r\n");
sype 121:0cc17ba879cb 433 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 434 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 435 asserv.setGoal(1,25);
sype 121:0cc17ba879cb 436 logger.printf("Attente\r\n");
sype 121:0cc17ba879cb 437 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 438 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 439 asserv.setGoal(1,30);
sype 121:0cc17ba879cb 440 logger.printf("Attente\r\n");
sype 121:0cc17ba879cb 441 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 442 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 443 asserv.setGoal(0,0,PI/2);
sype 121:0cc17ba879cb 444 logger.printf("Attente\r\n");
sype 121:0cc17ba879cb 445 while(!asserv.isArrived())wait(0.1);
Jagang 123:55e5e9acc541 446 logger.printf("Arrive\r\n");
sype 121:0cc17ba879cb 447 while(1);
sype 121:0cc17ba879cb 448 /*logger.printf("Arrivé\r\n");
sype 108:890094ee202a 449 wait(1);
sype 108:890094ee202a 450 asserv.setGoal(-300,300,0);
sype 108:890094ee202a 451 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 452 wait(1);
sype 108:890094ee202a 453 asserv.setGoal(-300,-1,0);
sype 108:890094ee202a 454 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 455 wait(1);
sype 108:890094ee202a 456 asserv.setGoal(-300,-300,0);
sype 108:890094ee202a 457 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 458 wait(1);
sype 108:890094ee202a 459 asserv.setGoal(1,-300,0);
sype 108:890094ee202a 460 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 461 wait(1);
sype 108:890094ee202a 462 asserv.setGoal(300,-300,0);
sype 108:890094ee202a 463 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 464 wait(1);
sype 121:0cc17ba879cb 465 asserv.setGoal(0,0,0);*/
Jagang 71:95d76c181b22 466 #endif
Near32 43:87bdce65341f 467 }
Jagang 93:4d5664e9188a 468
Jagang 93:4d5664e9188a 469 void update()
Jagang 93:4d5664e9188a 470 {
Jagang 93:4d5664e9188a 471 float dt = t.read();
Jagang 93:4d5664e9188a 472 t.reset();
Jagang 93:4d5664e9188a 473
Jagang 93:4d5664e9188a 474 odometry.update(dt);
Jagang 109:53918ba98306 475 asserv.update(dt);
Jagang 93:4d5664e9188a 476
Jagang 93:4d5664e9188a 477 #ifdef PLAN_A
Jagang 93:4d5664e9188a 478 float phi_r = (float)instanceAsserv.getPhiR();
Jagang 93:4d5664e9188a 479 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 93:4d5664e9188a 480 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 93:4d5664e9188a 481
Jagang 93:4d5664e9188a 482 motorR.setSpeed(0.08+(phi_r/phi_max));
Jagang 93:4d5664e9188a 483 motorL.setSpeed(0.08+(phi_l/phi_max));
Jagang 93:4d5664e9188a 484 #endif
Jagang 93:4d5664e9188a 485 }
Jagang 93:4d5664e9188a 486