File content as of revision 119:c45efcd706d9:

#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)
{
    limite = 0.5;
    cmd = 0;
    cmd_g = 0;
    cmd_d = 0;
    
    somme_erreur_theta = 0;
    delta_erreur_theta = 0;
    erreur_precedente_theta = 0;
    
    somme_erreur_distance = 0;
    delta_erreur_distance = 0;
    erreur_precedente_distance = 0;
    distanceGoal = 0;
    distance = 0;
    
    Kp_angle = 3.5; //Fixed à 3.0 pour 180 deg
    Ki_angle = 0.0;
    Kd_angle = 0.1;
    
    Kp_distance = 0.0040;
    Ki_distance = 0.00003;//0.000001
    Kd_distance = 0.0;//0.05
    
    N = 0;
    arrived = false;
    squip = false;
    state = 0; // Etat ou l'on ne fait rien
}

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

void aserv_planB::stop(void)
{
    m_motorL.setSpeed(0);
    m_motorR.setSpeed(0);
    /*m_goalX = m_odometry.getX();
    m_goalY = m_odometry.getY();
    setGoal(m_goalX, m_goalY);*/
}

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

void aserv_planB::update(float dt)
{
    if(state == 1) thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
    else if(state == 3) thetaGoal = m_goalPhi;
    float erreur_theta = thetaGoal-m_odometry.getTheta();
    
    float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
    
    delta_erreur_theta = erreur_theta - erreur_precedente_theta;
    erreur_precedente_theta = erreur_theta;
    somme_erreur_theta += erreur_theta;
    
    delta_erreur_distance = erreur_distance - erreur_precedente_distance;
    erreur_precedente_distance = erreur_distance;
    somme_erreur_distance += erreur_distance;
    
    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)
    {
        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
        cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
        
        if(cmd > limite) cmd = limite;
        else if(cmd < -limite) cmd = -limite;
        
        m_motorL.setSpeed(-cmd);
        m_motorR.setSpeed(cmd);
        
        if(abs(erreur_theta) < 0.05f) N++;
        else N = 0;
        if(N > 10)
        {
            m_motorL.setSpeed(0);
            m_motorR.setSpeed(0);
            state = 2;
            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
            somme_erreur_theta = 0;
            N = 0;
            Kp_angle = 3.5;
        }
    }

    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
    if(state == 2) 
    {
        if(delta_erreur_distance > 0) // On a dépassé le point
        {
            state = 1;
            return;
        }
        
        if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
        
        cmd_g = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance - (erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle);
        cmd_d = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance + erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
        
        
        if(abs(erreur_distance) > 55.0f)
        {
            if(cmd_g > limite) cmd_g = limite;
            else if(cmd_g < -limite) cmd_g = -limite;
            
            if(cmd_d > limite) cmd_d = limite;
            else if(cmd_d < -limite) cmd_d = -limite;
        }
        else
        {
            if(cmd_g > 0.2f) cmd_g = 0.2f;
            else if(cmd_g < -0.2f) cmd_g = -0.2f;
            
            if(cmd_d > 0.2f) cmd_d = 0.2f;
            else if(cmd_d < -0.2f) cmd_d = -0.2f;
        }
        
        if(cmd_g > 0.01f && cmd_g < 0.14f) cmd_g = 0.14f;
        if(cmd_g < -0.01f && cmd_g > -0.14f) cmd_g = -0.14f;
        
        if(cmd_d > 0.01f && cmd_d < 0.14f) cmd_d = 0.14f;
        if(cmd_d < -0.01f && cmd_d > -0.14f) cmd_d = -0.14f;
            
        m_motorL.setSpeed(cmd_g);
        m_motorR.setSpeed(cmd_d);
        
        if(abs(erreur_distance) < 10.0f) N++;
        else N = 0;
        if(N > 10)
        {
            logger.printf("Erreur distance : %.2f\r\n", erreur_distance);
            somme_erreur_distance = 0;
            state = 3;
            N = 0;
            Kp_angle = 3.5;
        }
    }

    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
    if(state == 3 && squip == false)
    {
        /*cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
        
        if(cmd > limite) cmd = limite;
        else if(cmd < -limite) cmd = -limite;
        
        m_motorL.setSpeed(-cmd);
        m_motorR.setSpeed(cmd);
        
        if(abs(erreur_theta)< 0.05) N++;
        else N = 0;
        if(N > 10)
        {
            m_motorL.setSpeed(0);
            m_motorR.setSpeed(0);
            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
            somme_erreur_theta = 0;
            N = 0;
            Kp_angle = 3.0;
            arrived = true;
        }*/
        m_motorL.setSpeed(0);
        m_motorR.setSpeed(0);
        arrived = true;
        state = 0;
    }
}