File content as of revision 117:f8c147141a0c:

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

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

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

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

void Obj_pince::run()
{
    float thetaGoal = atan2(y-odometry.getY(),x-odometry.getX());
    asserv.setGoal(odometry.getX()+cos(thetaGoal)*200,odometry.getY()+sin(thetaGoal)*200,thetaGoal); //On avance jusqu'au goblet/spot
    while(!asserv.isArrived())wait(0.1);
    
    ax12_pince->setGoal(PINCE_FERMEE);
    done = true;
    
    for(unsigned int i=0;i < objectifs.size();i++)
    {
        if(objectifs[i]->getId() == IDO_DEPOT && !objectifs[i]->isDone())
        {
            objectifs[i]->activate();
            break;
        }
    }
}

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