ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@109:53918ba98306, 2015-05-05 (annotated)

Committer:
Jagang
Date:
Tue May 05 16:35:53 2015 +0000
Revision:
109:53918ba98306
Parent:
103:6a38cc0765f5
Child:
110:7e71e5cd8197
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 109:53918ba98306 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 93:4d5664e9188a 156 ax12_pince.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 157 ax12_brasG.setMaxTorque(MAX_TORQUE);
Jagang 93:4d5664e9188a 158 ax12_brasD.setMaxTorque(MAX_TORQUE);
Jagang 39:09c04fd42c94 159
Jagang 93:4d5664e9188a 160 ax12_pince.setGoal(PINCE_FERMEE);
Jagang 93:4d5664e9188a 161 ax12_brasG.setGoal(BRASG_OUVERT);
Jagang 93:4d5664e9188a 162 ax12_brasD.setGoal(BRASD_OUVERT);
Jagang 103:6a38cc0765f5 163 wait(0.5);
Jagang 93:4d5664e9188a 164 ax12_pince.setGoal(PINCE_OUVERTE);
Jagang 93:4d5664e9188a 165 ax12_brasG.setGoal(BRASG_FERME);
Jagang 93:4d5664e9188a 166 ax12_brasD.setGoal(BRASD_FERME);
Jagang 93:4d5664e9188a 167 wait(1.5);
Jagang 93:4d5664e9188a 168 ax12_pince.setMaxTorque(0);
Jagang 93:4d5664e9188a 169 ax12_brasG.setMaxTorque(0);
Jagang 93:4d5664e9188a 170 ax12_brasD.setMaxTorque(0);
Jagang 93:4d5664e9188a 171
Jagang 93:4d5664e9188a 172 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 173
Jagang 93:4d5664e9188a 174 // //
Jagang 93:4d5664e9188a 175 // *--------------------------* //
Jagang 93:4d5664e9188a 176
Jagang 93:4d5664e9188a 177 // *--------------------------* //
Jagang 93:4d5664e9188a 178 // Asserv //
Jagang 93:4d5664e9188a 179
Jagang 93:4d5664e9188a 180 logger.printf("Demarrage asserv...........");
Jagang 93:4d5664e9188a 181 t.start();
Jagang 93:4d5664e9188a 182 ticker.attach_us(&update,10000); //100Hz
Jagang 93:4d5664e9188a 183 logger.printf("[done]\r\n");
Jagang 93:4d5664e9188a 184
Jagang 93:4d5664e9188a 185 // //
Jagang 93:4d5664e9188a 186 // *--------------------------* //
Jagang 109:53918ba98306 187
Jagang 109:53918ba98306 188 // *--------------------------* //
Jagang 109:53918ba98306 189 // Tirrette + couleur //
Jagang 109:53918ba98306 190
Jagang 109:53918ba98306 191 //while(tirette); // La tirrette
Jagang 109:53918ba98306 192
Jagang 109:53918ba98306 193 if(couleur == COULEUR_JAUNE)
Jagang 109:53918ba98306 194 {
Jagang 109:53918ba98306 195 couleurRobot = COULEUR_JAUNE;
Jagang 109:53918ba98306 196 objectifs.push_back(new Obj_clap(0, 0, 0, &asserv, &ax12_brasG,&ax12_brasD));
Jagang 109:53918ba98306 197 }
Jagang 109:53918ba98306 198 else
Jagang 109:53918ba98306 199 {
Jagang 109:53918ba98306 200 couleurRobot = COULEUR_VERTE;
Jagang 109:53918ba98306 201 }
Jagang 109:53918ba98306 202
Jagang 109:53918ba98306 203
Jagang 109:53918ba98306 204 // //
Jagang 109:53918ba98306 205 // *--------------------------* //
Jagang 93:4d5664e9188a 206
Jagang 93:4d5664e9188a 207 bool continuer = true;
Jagang 71:95d76c181b22 208
Jagang 109:53918ba98306 209 // *--------------------------* //
Jagang 109:53918ba98306 210 // IA //
Jagang 109:53918ba98306 211
Jagang 109:53918ba98306 212 logger.printf("Pathfinding ... ");
Jagang 109:53918ba98306 213 Timer t;
Jagang 109:53918ba98306 214 t.start();
Jagang 109:53918ba98306 215 int i = terrain.AStar(1000,300,1750,250,10);
Jagang 109:53918ba98306 216 t.stop();
Jagang 109:53918ba98306 217 logger.printf("[done]\r\n");
Jagang 109:53918ba98306 218
Jagang 109:53918ba98306 219 logger.printf("Return : %d in %.3fms\r\n",i,t.read()*1000);
Jagang 109:53918ba98306 220
Jagang 109:53918ba98306 221 for(i=0;i<terrain.path.size();i++)
Jagang 109:53918ba98306 222 {
Jagang 109:53918ba98306 223 logger.printf("%d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
Jagang 109:53918ba98306 224 //asserv.setGoal(terrain.path[i].x,terrain.path[i].y
Jagang 109:53918ba98306 225 }
Jagang 109:53918ba98306 226
Jagang 109:53918ba98306 227 while(1);
Jagang 109:53918ba98306 228
Jagang 109:53918ba98306 229 for(int x=-10;x<2000;x+=20)
Jagang 109:53918ba98306 230 {
Jagang 109:53918ba98306 231 for(int y=-10;y<3000;y+=20)
Jagang 109:53918ba98306 232 {
Jagang 109:53918ba98306 233 if(terrain.getHeight(x,y) >= 32000)
Jagang 109:53918ba98306 234 logger.printf("x");
Jagang 109:53918ba98306 235 else
Jagang 109:53918ba98306 236 logger.printf(" ");
Jagang 109:53918ba98306 237 }
Jagang 109:53918ba98306 238 logger.printf("\r\n");
Jagang 109:53918ba98306 239 }
Jagang 109:53918ba98306 240
Jagang 109:53918ba98306 241
Jagang 109:53918ba98306 242 while(continuer)
Jagang 109:53918ba98306 243 {
Jagang 109:53918ba98306 244 int objAct = 0;
Jagang 109:53918ba98306 245 bool newObj = false;
Jagang 109:53918ba98306 246 for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
Jagang 109:53918ba98306 247 {
Jagang 109:53918ba98306 248 if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
Jagang 109:53918ba98306 249 continue;
Jagang 109:53918ba98306 250
Jagang 109:53918ba98306 251 if(objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
Jagang 109:53918ba98306 252 continue;
Jagang 109:53918ba98306 253
Jagang 109:53918ba98306 254 if(terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getX(), 0.05) != 1) // L'objectif est atteignable !!
Jagang 109:53918ba98306 255 {
Jagang 109:53918ba98306 256 newObj = true;
Jagang 109:53918ba98306 257 break;
Jagang 109:53918ba98306 258 }
Jagang 109:53918ba98306 259 }
Jagang 109:53918ba98306 260
Jagang 109:53918ba98306 261 if(newObj)
Jagang 109:53918ba98306 262 {
Jagang 109:53918ba98306 263
Jagang 109:53918ba98306 264
Jagang 109:53918ba98306 265 }
Jagang 109:53918ba98306 266 else
Jagang 109:53918ba98306 267 {
Jagang 109:53918ba98306 268 logger.printf("Nothind to be done ... :( \r\n");
Jagang 109:53918ba98306 269 wait(1);
Jagang 109:53918ba98306 270 }
Jagang 109:53918ba98306 271
Jagang 109:53918ba98306 272 }
Jagang 109:53918ba98306 273
Jagang 71:95d76c181b22 274 #ifdef PLAN_A
Jagang 109:53918ba98306 275 asserv.setGoal(300.0f,200.0f,0.0f);
Near32 89:d05001d85a02 276 logger.printf("GOAL SET... RUNNING!\r\n");
Jagang 71:95d76c181b22 277
Jagang 74:88be86f83d17 278 char state = 0;
Jagang 74:88be86f83d17 279
Jagang 93:4d5664e9188a 280 while(continuer)
Jagang 71:95d76c181b22 281 {
Jagang 92:b403f2724252 282 #define test
Jagang 92:b403f2724252 283 #ifdef test
Jagang 109:53918ba98306 284 if(state == 0 && asserv.isArrivedRho())
Jagang 92:b403f2724252 285 {
Jagang 92:b403f2724252 286 state = 1;
Jagang 92:b403f2724252 287 logger.printf("Arrive en 0,0 !!!!!\r\n");
Near32 98:2ec4e17dfc92 288 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 289 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 290 wait(5);
Jagang 109:53918ba98306 291 asserv.setGoal(300.0f,200.0f,0.0f);
Jagang 92:b403f2724252 292 }
Jagang 109:53918ba98306 293 else if(state == 1 && asserv.isArrivedRho())
Jagang 92:b403f2724252 294 {
Jagang 92:b403f2724252 295 state = 2;
Jagang 92:b403f2724252 296 logger.printf("Arrive en 200,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 297 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 298 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 299 wait(5);
Jagang 109:53918ba98306 300 asserv.setGoal(0.0f,300.0f,0.0f);
Jagang 92:b403f2724252 301 }
Jagang 109:53918ba98306 302 else if(state == 2 && asserv.isArrivedRho())
Jagang 92:b403f2724252 303 {
Jagang 92:b403f2724252 304 state = 0;
Jagang 92:b403f2724252 305 logger.printf("Arrive en 0,200 !!!!!\r\n");
Near32 98:2ec4e17dfc92 306 motorR.setSpeed(0.0f);
Near32 98:2ec4e17dfc92 307 motorL.setSpeed(0.0f);
Jagang 92:b403f2724252 308 wait(5);
Jagang 109:53918ba98306 309 asserv.setGoal(0.0f,0.0f,0.0f);
Jagang 92:b403f2724252 310 }
Jagang 92:b403f2724252 311 #endif
Jagang 71:95d76c181b22 312 }
Jagang 71:95d76c181b22 313 #else
sype 85:8e95432d99d3 314 asserv.setGoal(45,45,0);
Jagang 71:95d76c181b22 315 #endif
Jagang 103:6a38cc0765f5 316
Jagang 103:6a38cc0765f5 317 while(1);
Near32 43:87bdce65341f 318 }
Jagang 93:4d5664e9188a 319
Jagang 93:4d5664e9188a 320 void update()
Jagang 93:4d5664e9188a 321 {
Jagang 93:4d5664e9188a 322 float dt = t.read();
Jagang 93:4d5664e9188a 323 t.reset();
Jagang 93:4d5664e9188a 324
Jagang 93:4d5664e9188a 325 odometry.update(dt);
Jagang 109:53918ba98306 326 asserv.update(dt);
Jagang 93:4d5664e9188a 327
Jagang 93:4d5664e9188a 328 #ifdef PLAN_A
Jagang 93:4d5664e9188a 329 float phi_r = (float)instanceAsserv.getPhiR();
Jagang 93:4d5664e9188a 330 float phi_l = (float)instanceAsserv.getPhiL();
Jagang 93:4d5664e9188a 331 float phi_max = (float)instanceAsserv.getPhiMax();
Jagang 93:4d5664e9188a 332
Jagang 93:4d5664e9188a 333 motorR.setSpeed(0.08+(phi_r/phi_max));
Jagang 93:4d5664e9188a 334 motorL.setSpeed(0.08+(phi_l/phi_max));
Jagang 93:4d5664e9188a 335 #endif
Jagang 93:4d5664e9188a 336 }
Jagang 93:4d5664e9188a 337