ARES
/
Robot 2016
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: mbed
Diff: main.cpp
- Revision:
- 0:b127c787a51b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,525 @@
+#include "includes.h"
+
+#include "Map.h"
+
+#include "Objectif.h"
+#include "Obj_clap.h"
+#include "Obj_pince.h"
+#include "Obj_depot.h"
+#include "Obj_recalage.h"
+
+#include "Obs_robot.h"
+
+#include "AX12.h"
+
+
+void update();
+void update_odo();
+
+// *--------------------------* //
+// Déclarations //
+
+// Decl. logger //
+
+Serial logger(OUT_TX, OUT_RX); // tx, rx
+
+// Decl. timer //
+
+Timer t;
+Ticker ticker;
+
+// Decl. AX12 //
+
+AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
+AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
+AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);
+
+// Decl. Moteurs //
+
+PwmOut pw1(PWM_MOT1);
+DigitalOut dir1(DIR_MOT1);
+PwmOut pw2(PWM_MOT2);
+DigitalOut dir2(DIR_MOT2);
+
+Motor motorL(PWM_MOT1,DIR_MOT1);
+Motor motorR(PWM_MOT2,DIR_MOT2);
+
+// Decl. Sharps //
+
+AnalogIn sharp_devant(SHARP_D);
+AnalogIn sharp_devant_droite(SHARP_DD);
+AnalogIn sharp_devant_gauche(SHARP_DG);
+AnalogIn sharp_arriere(SHARP_A);
+
+// Decl. Boutons //
+
+DigitalIn bp(BP_DESSUS);
+DigitalIn tirette(TIRETTE_DESSUS);
+DigitalIn couleur(COULEUR_DESSUS);
+
+// Decl. Odometrie //
+
+QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
+QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
+
+Odometry odometry(&qei_left,&qei_right,RAYONG,RAYOND,ENTREAXE);
+
+// Decl. Asserv //
+
+Asservissement *asserv = new Asservissement_v1(odometry,motorL,motorR);
+
+// Decl. IA //
+
+Map terrain;
+Timer timer;
+Timeout timeout;
+bool fini = false;
+bool interruption = false;
+std::vector<Objectif*> objectifs;
+char couleurRobot = COULEUR_JAUNE;
+
+void stop();
+
+// Fin Decl. //
+// *--------------------------* //
+
+
+int main()
+{
+ #ifdef OUT_USB
+ logger.baud((int)921600);
+ #endif
+
+ // *--------------------------* //
+ // Init //
+
+ logger.printf("[00.000] Initialisation.............");
+
+ // Init. AX12 //
+
+ ax12_pince.setMode(0);
+ ax12_brasG.setMode(0);
+ ax12_brasD.setMode(0);
+
+ // Init. Moteurs //
+
+ motorL.setSpeed(0);
+ motorR.setSpeed(0);
+
+ // Init. Sharps //
+
+ // Init. Boutons //
+
+ // Init. Odometrie //
+
+
+ odometry.setTheta(PI/2);
+ odometry.setX(0);
+ odometry.setY(0);
+
+ // Init. IA //
+
+ terrain.build();
+
+ logger.printf("[done]\r\n");
+
+ // Fin Init. //
+ // *--------------------------* //
+
+ // *--------------------------* //
+ // MIP //
+
+ logger.printf("Appuyer sur le bouton pour mettre en position\r\n");
+
+ while(!bp); // On attend le top de mise en position
+
+ // *--------------------------* //
+ // Asserv //
+
+ logger.printf("[00.000] Demarrage asserv!...........");
+ t.start();
+ ticker.attach_us(&update,10000); //100Hz/10ms
+ logger.printf("[done]\r\n");
+
+ timer.reset();
+ timer.start();
+
+ while(1);
+
+ // //
+ // *--------------------------* //
+
+ logger.printf("[00.000] MIP........................");
+
+ if(couleur == COULEUR_JAUNE)
+ {
+ couleurRobot = COULEUR_JAUNE;
+
+ odometry.setTheta(PI/2);
+ odometry.setX(1000);
+ odometry.setY(157);
+
+ asserv->setGoal(1000,400);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+ objectifs.push_back( new Obj_clap(1750, 650, PI/2, 1750, 880, PI/2, &ax12_brasG, &ax12_brasD) );
+
+ objectifs.push_back( new Obj_pince(1750, 250+350, 1750, 250+190, -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_PC2);
+
+ objectifs.push_back( new Obj_depot(1000, 350+1*100, -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_PC);
+
+ objectifs.push_back( new Obj_depot(1000, (350+2*100), -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+ objectifs.push_back( new Obj_depot(1100, (350+3*100), -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+
+ objectifs.push_back( new Obj_pince(1355-310, (870), 1355-170, (870), 0, &ax12_pince) );
+ objectifs.back()->setId(IDO_P4);
+
+ objectifs.push_back( new Obj_pince(1730, 350, 1730, 240, -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_P5);
+
+
+ objectifs.push_back( new Obj_clap(1750, 3000-2776, -PI/4, 1750, 3000-2630, PI/2, &ax12_brasG, &ax12_brasD) );
+
+ }
+ else
+ {
+ couleurRobot = COULEUR_VERTE;
+
+ odometry.setTheta(-PI/2);
+ odometry.setX(1000);
+ odometry.setY(3000-157);
+
+ asserv->setGoal(1000,3000-400);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+ objectifs.push_back( new Obj_clap(1750, 2350, -PI/2, 1750, 2100, -PI/2, &ax12_brasG, &ax12_brasD) );
+
+ objectifs.push_back( new Obj_pince(1750, 3000-(250+350), 1750, 3000-(250+190), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_PC4);
+
+ objectifs.push_back( new Obj_depot(1000, 3000-(350+1*100), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_PC);
+
+ objectifs.push_back( new Obj_depot(1000, 3000-(350+2*100), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+ objectifs.push_back( new Obj_depot(1100, 3000-(350+3*100), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+
+ objectifs.push_back( new Obj_pince(1355-330, 3000-(870), 1355-170, 3000-(870), 0, &ax12_pince) );
+ objectifs.back()->setId(IDO_P12);
+
+ objectifs.push_back( new Obj_pince(1730, 3000-350, 1730, 3000-240, PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_P13);
+
+ objectifs.push_back( new Obj_clap(1750, 2776, PI/4, 1750, 2630, -PI/2, &ax12_brasG, &ax12_brasD) );
+ }
+
+
+ ax12_pince.setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_brasG.setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_brasD.setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_pince.setGoal(PINCE_FERMEE);
+ wait(0.1);
+ ax12_brasG.setGoal(BRASG_OUVERT);
+ wait(0.1);
+ ax12_brasD.setGoal(BRASD_OUVERT);
+ wait(0.5);
+ ax12_pince.setGoal(PINCE_OUVERTE);
+ wait(0.1);
+ ax12_brasG.setGoal(BRASG_FERME);
+ wait(0.1);
+ ax12_brasD.setGoal(BRASD_FERME);
+
+ logger.printf("[done]\r\n");
+
+ // //
+ // *--------------------------* //
+
+ // *--------------------------* //
+ // Tirrette + couleur //
+
+ while(tirette) // La tirrette
+ {
+ logger.printf("Point actuel : %.3f\t%.3f\t%.3f\r\n",odometry.getX(), odometry.getY(), odometry.getTheta()*180/PI);
+ wait(0.3);
+ }
+ timer.reset();
+ timer.start();
+ timeout.attach(&stop,90); // On lance le chrono de 90s
+
+ // //
+ // *--------------------------* //
+
+ // *--------------------------* //
+ // IA //
+
+ while(!fini)
+ {
+ if(terrain.getHeight(odometry.getX(), odometry.getY()) >= 32000)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Collision, echappement.....");
+
+ // Réduction de la taille des obstacles
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ {
+ if(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
+ logger.printf("\r\n-> %d",terrain.obstacles[i]->getId());
+ while(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
+ terrain.obstacles[i]->setRobotRadius(terrain.obstacles[i]->getRobotRadius()-1);
+ }
+
+ logger.printf("[done]\r\n");
+ }
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Recherche d'objectif.......");
+ unsigned int objAct = 0;
+ bool newObj = false;
+ for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
+ {
+ if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
+ continue;
+ if(!objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
+ continue;
+ int retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 10);
+ if(retourAStar == 1) // L'objectif est atteignable !!
+ {
+ newObj = true;
+ break;
+ }
+
+ if(retourAStar == 3) // Overload memoire
+ retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 25);
+
+ if(retourAStar == 1) // L'objectif est atteignable !!
+ {
+ newObj = true;
+ break;
+ }
+
+ if(retourAStar == 3) // Overload memoire
+ retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 50);
+
+ if(retourAStar == 1) // L'objectif est atteignable !!
+ {
+ newObj = true;
+ break;
+ }
+ }
+
+ logger.printf("[done]\r\n");
+
+ // On remet la taille des obstacles
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ terrain.obstacles[i]->setRobotRadius(ROBOTRADIUS);
+
+ if(newObj)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("-> Objectif : %d\r\n",objectifs[objAct]->getId());
+
+ // Déplacement vers le nouvel objectif //
+
+ for(unsigned int i=1;i<terrain.path.size()-1;i++)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Point actuel : %.3f\t%.3f\t%.3f\r\n",odometry.getX(), odometry.getY(), odometry.getTheta()*180/PI);
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Prochain point : %d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
+
+ asserv->setGoal(terrain.path[i].x,terrain.path[i].y);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+ }
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Dernier point : %.3f\t%.3f\r\n",objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta()*180/PI);
+
+
+ asserv->setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+
+ // Execution de l'objectif //
+ if(!fini && !interruption)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Execution de l'objectif....");
+
+ objectifs[objAct]->run();
+ logger.printf("[done]\r\n");
+ }
+ }
+ else
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("-> Nothind to be done ... :D \r\n");
+
+ ax12_brasG.setMaxTorque(MAX_TORQUE);
+ ax12_brasD.setMaxTorque(MAX_TORQUE);
+
+ ax12_brasG.setGoal(155-30);
+ ax12_brasD.setGoal(BRASD_FERME);
+ wait(1);
+ ax12_brasG.setGoal(BRASG_FERME);
+ ax12_brasD.setGoal(145+30);
+ wait(1);
+ ax12_brasD.setGoal(BRASD_FERME);
+ wait(1);
+
+ ax12_brasG.setMaxTorque(0);
+ ax12_brasD.setMaxTorque(0);
+ }
+
+ interruption = false;
+ }
+
+ while(1);
+}
+
+#define MEAN 6
+
+void update()
+{
+ /*static float sharp[MEAN][3];
+ static int i_sharp = 0;
+ sharp[i_sharp][1] = sharp_devant.read()*3.3;
+ sharp[i_sharp][0] = sharp_devant_droite.read()*3.3;
+ sharp[i_sharp][2] = sharp_devant_gauche.read()*3.3;
+ i_sharp = (i_sharp+1)%MEAN;
+
+ float sharp_value[3];
+ for(unsigned int i=0;i<3;i++)
+ {
+ sharp_value[i] = 0;
+ for(unsigned int ii=0;ii<MEAN;ii++)
+ sharp_value[i] += sharp[ii][i];
+
+ sharp_value[i] = sharp_value[i] / MEAN;
+ }*/
+
+ float dt = t.read();
+ t.reset();
+
+ odometry.update(dt);
+ asserv->update(dt);
+
+ //logger.printf("%.3f|%.3f|%.3f\r\n",timer.read()*1000,odometry.getVitLeft(),odometry.getVitRight());
+
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ terrain.obstacles[i]->update(dt);
+
+ /*
+ #ifdef PLAN_A
+ float phi_r = (float)instanceasserv->getPhiR();
+ float phi_l = (float)instanceasserv->getPhiL();
+ float phi_max = (float)instanceasserv->getPhiMax();
+
+ motorR.setSpeed(0.08+(phi_r/phi_max));
+ motorL.setSpeed(0.08+(phi_l/phi_max));
+ #endif
+
+ bool sharpActif = true;
+ static float lastTime = -1;
+
+ // Distributeur
+ if(odometry.getX() <= 600 && ((-PI <= odometry.getTheta() && odometry.getTheta() <= -3*PI/4) || (3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= PI) ))
+ sharpActif = false;
+
+ if(odometry.getX() >= 1300 && -PI/4 <= odometry.getTheta() && odometry.getTheta() <= PI/4)
+ sharpActif = false;
+
+ if(odometry.getY() <= 650 && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+
+ if(odometry.getY() >= 2300 && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+
+ if(couleur == COULEUR_JAUNE)
+ {
+ if(700 <= odometry.getX() && odometry.getX() <= 1300 && odometry.getY() <= 1000 && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+ }
+ else
+ {
+ if(700 <= odometry.getX() && odometry.getX() <= 1300 && 2000 <= odometry.getY() && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+ }
+
+ if(odometry.getX() >= 1400 && PI/4 <= odometry.getTheta() && odometry.getTheta() <= 3*PI/4)
+ sharp_value[0] = 0;
+
+ if(odometry.getX() >= 1400 && -2*PI/6 <= odometry.getTheta() && odometry.getTheta() <= 2*PI/6)
+ {
+ sharp_value[2] = 0;
+ sharp_value[1] = 0;
+ }
+
+ if(timer.read() < 5)
+ sharpActif = false;
+
+ if((sharp_value[0] >= 0.9 || sharp_value[1] >= 0.9 || sharp_value[2] >= 0.9 ) && asserv->getState() == 2 && sharpActif) // Quelque chose devant et on avance!!
+ {
+ motorL.setSpeed(0);
+ motorR.setSpeed(0);
+ if(lastTime == -1)
+ lastTime = timer.read();
+
+ if(timer.read() - lastTime > 2) //Si on est bloqué depuis 2 secondes
+ {
+ float x = odometry.getX() + 400*cos(odometry.getTheta());
+ float y = odometry.getX() + 400*sin(odometry.getTheta());
+ terrain.obstacles.push_back(new Obs_robot(ROBOTRADIUS,IDO_ROBOT,x,y,300,10));
+
+ interruption = true;
+ asserv->stop();
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Evitement\r\n");
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Point actuel : %.3f\t%.3f\t%.3f\r\n",odometry.getX(), odometry.getY(), odometry.getTheta()*180/PI);
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Mechant robot : %.3f\t%.3f\r\n",x, y);
+ }
+ }
+ else
+ {
+ lastTime = -1;
+ } */
+}
+
+void stop()
+{
+ fini = true;
+ interruption = true;
+ asserv->stop();
+
+ ax12_brasG.setMaxTorque(MAX_TORQUE);
+ ax12_brasD.setMaxTorque(MAX_TORQUE);
+ ax12_pince.setMaxTorque(MAX_TORQUE);
+
+ ax12_brasG.setGoal(BRASG_FERME);
+ ax12_brasD.setGoal(BRASD_FERME);
+ ax12_pince.setGoal(PINCE_OUVERTE);
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Fin des 90 secondes !");
+
+ wait(2);
+
+ fini = true;
+ interruption = true;
+ asserv->stop();
+
+ ax12_brasG.setMaxTorque(0);
+ ax12_brasD.setMaxTorque(0);
+ ax12_pince.setMaxTorque(0);
+}
