ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Map/Objectifs/Obj_pince.cpp@119:c45efcd706d9, 2015-05-06 (annotated)

Committer:
Jagang
Date:
Wed May 06 15:17:16 2015 +0000
Revision:
119:c45efcd706d9
Parent:
117:f8c147141a0c
Child:
123:55e5e9acc541
Modif asserv B; Impl?mentation de l'IA et des objectifs (Ramasser PC et d?poser)

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 117:f8c147141a0c 3
Jagang 117:f8c147141a0c 4 #ifdef PLAN_A
Jagang 117:f8c147141a0c 5 extern Asserv<float> asserv;
Jagang 117:f8c147141a0c 6 #else
Jagang 117:f8c147141a0c 7 extern aserv_planB asserv;
Jagang 117:f8c147141a0c 8 #endif
Jagang 117:f8c147141a0c 9
Jagang 117:f8c147141a0c 10 extern Odometry2 odometry;
Jagang 117:f8c147141a0c 11 extern std::vector<Objectif*> objectifs;
Jagang 117:f8c147141a0c 12
Jagang 119:c45efcd706d9 13 Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
Jagang 117:f8c147141a0c 14 :Objectif(x,y,theta)
Jagang 117:f8c147141a0c 15 {
Jagang 117:f8c147141a0c 16 this->ax12_pince = ax12_pince;
Jagang 119:c45efcd706d9 17 this->xp = xp;
Jagang 119:c45efcd706d9 18 this->yp = yp;
Jagang 117:f8c147141a0c 19 }
Jagang 117:f8c147141a0c 20
Jagang 117:f8c147141a0c 21 void Obj_pince::run()
Jagang 117:f8c147141a0c 22 {
Jagang 119:c45efcd706d9 23 asserv.setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
Jagang 117:f8c147141a0c 24 while(!asserv.isArrived())wait(0.1);
Jagang 117:f8c147141a0c 25
Jagang 119:c45efcd706d9 26 ax12_pince->setMaxTorque(MAX_TORQUE);
Jagang 117:f8c147141a0c 27 ax12_pince->setGoal(PINCE_FERMEE);
Jagang 119:c45efcd706d9 28 wait(1.5);
Jagang 117:f8c147141a0c 29 done = true;
Jagang 117:f8c147141a0c 30
Jagang 117:f8c147141a0c 31 for(unsigned int i=0;i < objectifs.size();i++)
Jagang 117:f8c147141a0c 32 {
Jagang 117:f8c147141a0c 33 if(objectifs[i]->getId() == IDO_DEPOT && !objectifs[i]->isDone())
Jagang 117:f8c147141a0c 34 {
Jagang 117:f8c147141a0c 35 objectifs[i]->activate();
Jagang 117:f8c147141a0c 36 break;
Jagang 117:f8c147141a0c 37 }
Jagang 117:f8c147141a0c 38 }
Jagang 117:f8c147141a0c 39 }
Jagang 117:f8c147141a0c 40
Jagang 117:f8c147141a0c 41 int Obj_pince::isActive()
Jagang 117:f8c147141a0c 42 {
Jagang 117:f8c147141a0c 43 if(!active)
Jagang 117:f8c147141a0c 44 return false;
Jagang 117:f8c147141a0c 45
Jagang 117:f8c147141a0c 46 if(ax12_pince->getGoal() == PINCE_FERMEE)
Jagang 117:f8c147141a0c 47 return false;
Jagang 117:f8c147141a0c 48
Jagang 117:f8c147141a0c 49 return true;
Jagang 117:f8c147141a0c 50 }