ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

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

Committer:
Jagang
Date:
Wed May 06 11:22:17 2015 +0000
Revision:
117:f8c147141a0c
Parent:
114:be06d518b4a7
Child:
119:c45efcd706d9
Objectif pince et depot

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 117:f8c147141a0c 13 Obj_pince::Obj_pince(float x, float y, 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 117:f8c147141a0c 17 }
Jagang 117:f8c147141a0c 18
Jagang 117:f8c147141a0c 19 void Obj_pince::run()
Jagang 117:f8c147141a0c 20 {
Jagang 117:f8c147141a0c 21 float thetaGoal = atan2(y-odometry.getY(),x-odometry.getX());
Jagang 117:f8c147141a0c 22 asserv.setGoal(odometry.getX()+cos(thetaGoal)*200,odometry.getY()+sin(thetaGoal)*200,thetaGoal); //On avance jusqu'au goblet/spot
Jagang 117:f8c147141a0c 23 while(!asserv.isArrived())wait(0.1);
Jagang 117:f8c147141a0c 24
Jagang 117:f8c147141a0c 25 ax12_pince->setGoal(PINCE_FERMEE);
Jagang 117:f8c147141a0c 26 done = true;
Jagang 117:f8c147141a0c 27
Jagang 117:f8c147141a0c 28 for(unsigned int i=0;i < objectifs.size();i++)
Jagang 117:f8c147141a0c 29 {
Jagang 117:f8c147141a0c 30 if(objectifs[i]->getId() == IDO_DEPOT && !objectifs[i]->isDone())
Jagang 117:f8c147141a0c 31 {
Jagang 117:f8c147141a0c 32 objectifs[i]->activate();
Jagang 117:f8c147141a0c 33 break;
Jagang 117:f8c147141a0c 34 }
Jagang 117:f8c147141a0c 35 }
Jagang 117:f8c147141a0c 36 }
Jagang 117:f8c147141a0c 37
Jagang 117:f8c147141a0c 38 int Obj_pince::isActive()
Jagang 117:f8c147141a0c 39 {
Jagang 117:f8c147141a0c 40 if(!active)
Jagang 117:f8c147141a0c 41 return false;
Jagang 117:f8c147141a0c 42
Jagang 117:f8c147141a0c 43 if(ax12_pince->getGoal() == PINCE_FERMEE)
Jagang 117:f8c147141a0c 44 return false;
Jagang 117:f8c147141a0c 45
Jagang 117:f8c147141a0c 46 return true;
Jagang 117:f8c147141a0c 47 }