ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Map/Objectifs/Obj_pince.cpp@123:55e5e9acc541, 2015-05-11 (annotated)

Committer:
Jagang
Date:
Mon May 11 20:32:11 2015 +0000
Revision:
123:55e5e9acc541
Parent:
119:c45efcd706d9 
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 117:f8c147141a0c 1 #include "Obj_pince.h"
Jagang 117:f8c147141a0c 2 #include <vector>
Jagang 123:55e5e9acc541 3 #include "Map.h"
Jagang 117:f8c147141a0c 4
Jagang 117:f8c147141a0c 5 #ifdef PLAN_A
Jagang 117:f8c147141a0c 6 extern Asserv<float> asserv;
Jagang 117:f8c147141a0c 7 #else
Jagang 117:f8c147141a0c 8 extern aserv_planB asserv;
Jagang 117:f8c147141a0c 9 #endif
Jagang 117:f8c147141a0c 10
Jagang 117:f8c147141a0c 11 extern Odometry2 odometry;
Jagang 117:f8c147141a0c 12 extern std::vector<Objectif*> objectifs;
Jagang 123:55e5e9acc541 13 extern Map terrain;
Jagang 117:f8c147141a0c 14
Jagang 119:c45efcd706d9 15 Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
Jagang 117:f8c147141a0c 16 :Objectif(x,y,theta)
Jagang 117:f8c147141a0c 17 {
Jagang 117:f8c147141a0c 18 this->ax12_pince = ax12_pince;
Jagang 119:c45efcd706d9 19 this->xp = xp;
Jagang 119:c45efcd706d9 20 this->yp = yp;
Jagang 117:f8c147141a0c 21 }
Jagang 117:f8c147141a0c 22
Jagang 117:f8c147141a0c 23 void Obj_pince::run()
Jagang 117:f8c147141a0c 24 {
Jagang 119:c45efcd706d9 25 asserv.setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
Jagang 117:f8c147141a0c 26 while(!asserv.isArrived())wait(0.1);
Jagang 117:f8c147141a0c 27
Jagang 119:c45efcd706d9 28 ax12_pince->setMaxTorque(MAX_TORQUE);
Jagang 117:f8c147141a0c 29 ax12_pince->setGoal(PINCE_FERMEE);
Jagang 119:c45efcd706d9 30 wait(1.5);
Jagang 117:f8c147141a0c 31 done = true;
Jagang 117:f8c147141a0c 32
Jagang 117:f8c147141a0c 33 for(unsigned int i=0;i < objectifs.size();i++)
Jagang 117:f8c147141a0c 34 {
Jagang 123:55e5e9acc541 35 if(IDO_PC1 <= id && id <= IDO_PC4 && objectifs[i]->getId() == IDO_DEPOT_PC && !objectifs[i]->isDone())
Jagang 123:55e5e9acc541 36 {
Jagang 123:55e5e9acc541 37 objectifs[i]->activate();
Jagang 123:55e5e9acc541 38 break;
Jagang 123:55e5e9acc541 39 }
Jagang 123:55e5e9acc541 40 if(IDO_P1 <= id && id <= IDO_P16 && objectifs[i]->getId() == IDO_DEPOT_P && !objectifs[i]->isDone())
Jagang 117:f8c147141a0c 41 {
Jagang 117:f8c147141a0c 42 objectifs[i]->activate();
Jagang 117:f8c147141a0c 43 break;
Jagang 117:f8c147141a0c 44 }
Jagang 117:f8c147141a0c 45 }
Jagang 123:55e5e9acc541 46
Jagang 123:55e5e9acc541 47 for(unsigned int i=0;i < terrain.obstacles.size();i++)
Jagang 123:55e5e9acc541 48 if(id == terrain.obstacles[i]->getId())
Jagang 123:55e5e9acc541 49 terrain.obstacles[i]->desactivate();
Jagang 117:f8c147141a0c 50 }
Jagang 117:f8c147141a0c 51
Jagang 117:f8c147141a0c 52 int Obj_pince::isActive()
Jagang 117:f8c147141a0c 53 {
Jagang 117:f8c147141a0c 54 if(!active)
Jagang 117:f8c147141a0c 55 return false;
Jagang 117:f8c147141a0c 56
Jagang 117:f8c147141a0c 57 if(ax12_pince->getGoal() == PINCE_FERMEE)
Jagang 117:f8c147141a0c 58 return false;
Jagang 117:f8c147141a0c 59
Jagang 117:f8c147141a0c 60 return true;
Jagang 117:f8c147141a0c 61 }