File content as of revision 123:55e5e9acc541:

#include "Obj_pince.h"
#include <vector>
#include "Map.h"

#ifdef PLAN_A
    extern Asserv<float> asserv;
#else
    extern aserv_planB asserv;
#endif

extern Odometry2 odometry;
extern std::vector<Objectif*> objectifs;
extern Map terrain;

Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
:Objectif(x,y,theta)
{
    this->ax12_pince = ax12_pince;
    this->xp = xp;
    this->yp = yp;
}

void Obj_pince::run()
{
    asserv.setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
    while(!asserv.isArrived())wait(0.1);
    
    ax12_pince->setMaxTorque(MAX_TORQUE);
    ax12_pince->setGoal(PINCE_FERMEE);
    wait(1.5);
    done = true;
    
    for(unsigned int i=0;i < objectifs.size();i++)
    {
        if(IDO_PC1 <= id && id <= IDO_PC4 && objectifs[i]->getId() == IDO_DEPOT_PC && !objectifs[i]->isDone())
        {
            objectifs[i]->activate();
            break;
        }
        if(IDO_P1 <= id && id <= IDO_P16 && objectifs[i]->getId() == IDO_DEPOT_P && !objectifs[i]->isDone())
        {
            objectifs[i]->activate();
            break;
        }
    }
    
    for(unsigned int i=0;i < terrain.obstacles.size();i++)
        if(id == terrain.obstacles[i]->getId())
            terrain.obstacles[i]->desactivate();
}

int Obj_pince::isActive()
{
    if(!active)
        return false;
    
    if(ax12_pince->getGoal() == PINCE_FERMEE)
        return false;
    
    return true;
}