File content as of revision 18:0f1fefe78266:

#include "Obj_clap.h"

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

Obj_clap::Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD)
:Objectif(x,y,theta)
{
    this->ax12_brasG = ax12_brasG;
    this->ax12_brasD = ax12_brasD;
    this->x2 = x2;
    this->y2 = y2;
    this->theta2 = theta2;
}

void Obj_clap::run()
{
    if(theta2 == PI/2)
    {
        ax12_brasD->setMaxTorque(MAX_TORQUE);
        ax12_brasD->setGoal(BRASD_OUVERT);
        wait(1);
        asserv.setGoal(x2,y2,theta2);
        while(!asserv.isArrived())wait(0.1);
        ax12_brasD->setGoal(BRASD_FERME);
        wait(1);
        ax12_brasD->setMaxTorque(0);
    }
    else
    {
        ax12_brasG->setMaxTorque(MAX_TORQUE);
        ax12_brasG->setGoal(BRASG_OUVERT);
        wait(1);
        asserv.setGoal(x2,y2,theta2);
        while(!asserv.isArrived())wait(0.1);
        ax12_brasG->setGoal(BRASG_FERME);
        wait(1);
        ax12_brasG->setMaxTorque(0);
    }
    
    done = true;
}

int Obj_clap::isActive()
{
    if(!active)
        return false;
    
    if(ax12_brasG->getGoal() == BRASG_OUVERT || ax12_brasD->getGoal() == BRASD_OUVERT)
        return false;
    
    return true;
}