File content as of revision 106:05096985d1b2:

#include "planB.h"
#include "defines.h"

extern Serial logger;

aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
{
    erreur_precedente = 0;
    somme_erreur = 0;
    Kp_angle = 3.0; //Fixed à 3.0 pour 180 deg
    Ki_angle = 0.0;
    Kd_angle = 0.0;
    cmd = 0;
    cmd_g = 0, cmd_d = 0;
    N = 0;
    done = false;
    squip = false;
    state = 0; // Etat ou l'on ne fait rien
    distanceGoal = 0;
    distance = 0;
    Kp_distance = 0.004;
    Ki_distance = 0;
    Kd_distance = 0;
}

void aserv_planB::setGoal(float x, float y, float phi)
{
    m_goalX = x;
    m_goalY = y;
    m_goalPhi = phi;
    distanceGoal = sqrt(carre(m_goalY-m_odometry.getY())+carre(m_goalX-m_odometry.getX()));
    state = 3; // Etat de rotation 1
    N = 0;
}

void aserv_planB::setGoal(float x, float y)
{
    squip = true;
    setGoal(x, y, 0);
    squip = false;
}

void aserv_planB::update(float dt)
{      
    
    thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
    float erreur_theta = thetaGoal-m_odometry.getTheta();
    if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
    if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
    
    // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
    if(state == 1 && N < 120)
    {   
        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
        
        cmd = erreur_theta*Kp_angle + (erreur_theta-erreur_precedente)*Kd_angle + somme_erreur*Ki_angle;
        erreur_precedente = erreur_theta;
        somme_erreur += erreur_theta;
        
        m_motorL.setSpeed(-cmd);
        m_motorR.setSpeed(cmd);

        N++;
        
        if(N==120) // && (abs(erreur_theta)<=2.0)
        {   
            state = 2;
            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
            N = 0;
            /*m_odometry.setDistLeft(0);
            m_odometry.setDistRight(0);
            memo_g = m_odometry.getDistLeft();
            memo_d = m_odometry.getDistRight();*/
            
        }
    }
    
    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
    if(state == 2)
    {    
        float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
        //logger.printf("%.2f\r\n", erreur_distance);
        
        cmd_g = erreur_distance*Kp_distance - erreur_theta*Kp_angle;
        cmd_d = erreur_distance*Kp_distance + erreur_theta*Kp_angle;
        m_motorL.setSpeed(cmd_g);
        m_motorR.setSpeed(cmd_d);
        
        N++;
        
        if(abs(erreur_distance) <= 3.5 || N > 500) 
        {
            state = 3;
            logger.printf("Erreur de distance : %.1f\r\n", erreur_distance);
        }
    }
    
    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
    if(state == 3 && squip==false)
    {
        float erreur_phi = m_goalPhi-m_odometry.getTheta();
        logger.printf("%.2f\r\n", erreur_phi);
        m_motorL.setSpeed(0);
        m_motorR.setSpeed(0);
        done = true;
    }
}


/*float erreur_distance_g = distance_g-(m_odometry.getDistLeft()-memo_g); //- distance parcourue par la roue gauche depuis l'état 2
float erreur_distance_d = distance_d-(m_odometry.getDistRight()-memo_d);
cmd_g = erreur_distance_g*Kp_distance;
cmd_d = erreur_distance_d*Kp_distance;
m_motorL.setSpeed(cmd_g);
m_motorR.setSpeed(cmd_d);*/
        
/*void aserv_planB::control_speed()
{
    vitesse_d = m_odometry.getVitRight();
    vitesse_g = m_odometry.getVitLeft();

    erreur_g = consigne_g - vitesse_g;
    cmd_g = erreur_g*Kp;
    erreur_d = consigne_d - vitesse_d;
    cmd_d = erreur_d*Kp;

    m_motorL.setSpeed(cmd_g);
    m_motorR.setSpeed(cmd_d);
}*/