Obj_pince.cpp

00001 #include "Obj_pince.h"
00002 #include <vector>
00003 #include "Map.h"
00004 
00005 extern Asservissement *asserv;
00006 
00007 extern Motor motorL;
00008 extern Motor motorR;
00009 extern Odometry odometry;
00010 extern std::vector<Objectif*> objectifs;
00011 extern Map terrain;
00012 extern bool interruption;
00013 
00014 Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
00015 :Objectif(x,y,theta)
00016 {
00017     this->ax12_pince = ax12_pince;
00018     this->xp = xp;
00019     this->yp = yp;
00020 }
00021 
00022 void Obj_pince::run()
00023 {
00024     //if(x!=xp && y!=yp)
00025     //{
00026         if(!interruption) asserv->setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
00027         while(!asserv->isArrived() && !interruption)wait(0.1);
00028     //}
00029     ax12_pince->setMaxTorque(MAX_TORQUE);
00030     wait(0.1);
00031     ax12_pince->setGoal(PINCE_FERMEE);
00032     wait(1.5);
00033     
00034     done = true;
00035     
00036     for(unsigned int i=0;i < objectifs.size();i++)
00037     {
00038         if(IDO_PC1 <= id && id <= IDO_PC5 && objectifs[i]->getId() == IDO_DEPOT_PC && !objectifs[i]->isDone())
00039         {
00040             objectifs[i]->activate();
00041             break;
00042         }
00043         if(IDO_P1 <= id && id <= IDO_P16 && objectifs[i]->getId() == IDO_DEPOT_P && !objectifs[i]->isDone())
00044         {
00045             objectifs[i]->activate();
00046             break;
00047         }
00048     }
00049     
00050     for(unsigned int i=0;i < terrain.obstacles.size();i++)
00051         if(id == terrain.obstacles[i]->getId())
00052             terrain.obstacles[i]->desactivate();
00053 }
00054 
00055 int Obj_pince::isActive()
00056 {
00057     if(!active)
00058         return false;
00059     
00060     if(ax12_pince->getGoal() == PINCE_FERMEE)
00061         return false;
00062     
00063     return true;
00064 }