File content as of revision 0:b127c787a51b:

#include "Obj_recalage.h"

extern Motor motorL;
extern Motor motorR;
extern Odometry odometry;
extern bool interruption;

Obj_recalage::Obj_recalage(float x, float y, float theta)
:Objectif(x,y,theta)
{
    
}

void Obj_recalage::run()
{
    motorL.setSpeed(-0.3);
    motorR.setSpeed(-0.3);
    
    float lastPos = 0;
    if(theta == -PI/2)
    {
        lastPos = odometry.getY();
        while(abs(lastPos-odometry.getY()) > 2) // tant qu'on bouge
        {
            lastPos = odometry.getY();
        }
        // On a finit
        if(abs((3000-82.2)-odometry.getY()) < 200) // bonne erreur, on prend en compte
        {
            odometry.setY(3000-82.2f);
            odometry.setTheta(theta);
        }
    }
    
    if(theta == PI/2)
    {
        lastPos = odometry.getY();
        while(abs(lastPos-odometry.getY()) > 2) // tant qu'on bouge
        {
            lastPos = odometry.getY();
        }
        // On a finit
        if(abs(82.2f-odometry.getY()) < 200.0f) // bonne erreur, on prend en compte
        {
            odometry.setY(82.2f);
            odometry.setTheta(theta);
        }
    }
    
    motorL.setSpeed(0);
    motorR.setSpeed(0);
    
    wait(0.1);
    
    done = true;
}

int Obj_recalage::isActive()
{
    if(!active)
        return false;
    return true;
}