File content as of revision 0:b127c787a51b:

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

extern Asservissement *asserv;

extern Motor motorL;
extern Motor motorR;
extern Odometry odometry;
extern std::vector<Objectif*> objectifs;
extern Map terrain;
extern bool interruption;

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()
{
    //if(x!=xp && y!=yp)
    //{
        if(!interruption) asserv->setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
        while(!asserv->isArrived() && !interruption)wait(0.1);
    //}
    ax12_pince->setMaxTorque(MAX_TORQUE);
    wait(0.1);
    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_PC5 && 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;
}