ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@110:7e71e5cd8197, 2015-05-05 (annotated)

Committer:
Jagang
Date:
Tue May 05 16:52:09 2015 +0000
Revision:
110:7e71e5cd8197
Parent:
109:53918ba98306
Parent:
108:890094ee202a
Child:
111:c8a1129691da
Merging + test IA

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
Jagang 103:6a38cc0765f5 74 Odometry 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
Jagang 109:53918ba98306 103 logger.baud((int)9600);
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
Jagang 110:7e71e5cd8197 221 logger.printf("Pathfinding ... ");
Jagang 110:7e71e5cd8197 222 Timer t;
Jagang 110:7e71e5cd8197 223 t.start();
Jagang 110:7e71e5cd8197 224 int i = terrain.AStar(1000,300,1750,450,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
Jagang 110:7e71e5cd8197 230 for(i=0;i<terrain.path.size();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 110:7e71e5cd8197 236
Jagang 110:7e71e5cd8197 237 logger.printf("Finit !! \r\n");
Jagang 110:7e71e5cd8197 238
Jagang 110:7e71e5cd8197 239 while(1);
Jagang 109:53918ba98306 240
Jagang 110:7e71e5cd8197 241 #endif
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 109:53918ba98306 264
Jagang 109:53918ba98306 265
Jagang 109:53918ba98306 266 }
Jagang 109:53918ba98306 267 else
Jagang 109:53918ba98306 268 {
Jagang 109:53918ba98306 269 logger.printf("Nothind to be done ... :( \r\n");
Jagang 109:53918ba98306 270 wait(1);
Jagang 109:53918ba98306 271 }
Jagang 109:53918ba98306 272
Jagang 109:53918ba98306 273 }
Jagang 109:53918ba98306 274
Jagang 71:95d76c181b22 275 #ifdef PLAN_A
Jagang 109:53918ba98306 276 asserv.setGoal(300.0f,200.0f,0.0f);
Near32 89:d05001d85a02 277 logger.printf("GOAL SET... RUNNING!\r\n");
Jagang 71:95d76c181b22 278
Jagang 74:88be86f83d17 279 char state = 0;
Jagang 74:88be86f83d17 280
Jagang 93:4d5664e9188a 281 while(continuer)
Jagang 71:95d76c181b22 282 {
Jagang 92:b403f2724252 283 #define test
Jagang 92:b403f2724252 284 #ifdef test
Jagang 109:53918ba98306 285 if(state == 0 && asserv.isArrivedRho())
Jagang 92:b403f2724252 286 {
Jagang 92:b403f2724252 287 state = 1;
Jagang 92:b403f2724252 288 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 98:2ec4e17dfc92 289 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 290 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 291 wait(5);
Jagang 109:53918ba98306 292 asserv.setGoal(300.0f,200.0f,0.0f);
Jagang 92:b403f2724252 293 }
Jagang 109:53918ba98306 294 else if(state == 1 && asserv.isArrivedRho())
Jagang 92:b403f2724252 295 {
Jagang 92:b403f2724252 296 state = 2;
Jagang 92:b403f2724252 297 logger.printf("Arrive en 200,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 298 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 299 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 300 wait(5);
Jagang 109:53918ba98306 301 asserv.setGoal(0.0f,300.0f,0.0f);
Jagang 92:b403f2724252 302 }
Jagang 109:53918ba98306 303 else if(state == 2 && asserv.isArrivedRho())
Jagang 92:b403f2724252 304 {
Jagang 92:b403f2724252 305 state = 0;
Jagang 92:b403f2724252 306 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 307 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 308 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 309 wait(5);
Jagang 109:53918ba98306 310 asserv.setGoal(0.0f,0.0f,0.0f);
Jagang 92:b403f2724252 311 }
Jagang 92:b403f2724252 312 #endif
Jagang 71:95d76c181b22 313 }
Jagang 71:95d76c181b22 314 #else
sype 108:890094ee202a 315 asserv.setGoal(1,300,0);
sype 108:890094ee202a 316 logger.printf("Attente\r\n");
sype 108:890094ee202a 317 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 318 logger.printf("Arrivé\r\n");
sype 108:890094ee202a 319 wait(1);
sype 108:890094ee202a 320 asserv.setGoal(-300,300,0);
sype 108:890094ee202a 321 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 322 wait(1);
sype 108:890094ee202a 323 asserv.setGoal(-300,-1,0);
sype 108:890094ee202a 324 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 325 wait(1);
sype 108:890094ee202a 326 asserv.setGoal(-300,-300,0);
sype 108:890094ee202a 327 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 328 wait(1);
sype 108:890094ee202a 329 asserv.setGoal(1,-300,0);
sype 108:890094ee202a 330 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 331 wait(1);
sype 108:890094ee202a 332 asserv.setGoal(300,-300,0);
sype 108:890094ee202a 333 while(!asserv.isArrived())wait(0.1);
sype 108:890094ee202a 334 wait(1);
sype 108:890094ee202a 335 asserv.setGoal(0,0,0);
Jagang 71:95d76c181b22 336 #endif
Jagang 103:6a38cc0765f5 337
Jagang 103:6a38cc0765f5 338 while(1);
Near32 43:87bdce65341f 339 }
Jagang 93:4d5664e9188a 340
Jagang 93:4d5664e9188a 341 void update()
Jagang 93:4d5664e9188a 342 {
Jagang 93:4d5664e9188a 343 float dt = t.read();
Jagang 93:4d5664e9188a 344 t.reset();
Jagang 93:4d5664e9188a 345
Jagang 93:4d5664e9188a 346 odometry.update(dt);
Jagang 109:53918ba98306 347 asserv.update(dt);
Jagang 93:4d5664e9188a 348
Jagang 93:4d5664e9188a 349 #ifdef PLAN_A
Jagang 93:4d5664e9188a 350 float phi_r = (float)instanceAsserv.getPhiR();
Jagang 93:4d5664e9188a 351 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 93:4d5664e9188a 352 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 93:4d5664e9188a 353
Jagang 93:4d5664e9188a 354 motorR.setSpeed(0.08+(phi_r/phi_max));
Jagang 93:4d5664e9188a 355 motorL.setSpeed(0.08+(phi_l/phi_max));
Jagang 93:4d5664e9188a 356 #endif
Jagang 93:4d5664e9188a 357 }
Jagang 93:4d5664e9188a 358