File content as of revision 100:a827a645d6c2:

#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)
{
    consigne_g = 0.0;
    consigne_d = 0.0;
    vitesse_g = 0;
    vitesse_d = 0;
    erreur_g = 0;
    erreur_d = 0;
    cmd_g = 0;
    cmd_d = 0;
    somme_erreur = 0;
    somme_erreur_d = 0;
    delta_erreur = 0;
    erreur_precedente = 0;
    Kp = 0.30;
    Ki = 0.01;
    Kd = 0.5;
    //etat_angle = 0;
    cmd = 0;
    N = 0;
    moyenne = 0;
    limite = 0;
    done = false;
    state = 0; // Etat ou l'on ne fait rien
}

void aserv_planB::setGoal(float x, float y, float theta)
{
    m_goalX = x;
    m_goalY = y;
    m_goalTheta = theta;

    state = 1; // Etat de rotation 1
}

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);
}

void aserv_planB::update(float dt)
{
    if(state == 1)
    {
        float 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;
        
        /*if(erreur_theta < 0) etat_angle = 1;
        else if(erreur_theta > 0) etat_angle = 2;
        else etat_angle = 0;*/
        /*limite = (0.5-Kp*erreur_theta)/Ki;
        if(somme_erreur >= limite) somme_erreur = limite;
        if(somme_erreur <= -limite) somme_erreur = -limite;*/
        
        cmd = erreur_theta*Kp + somme_erreur*Ki - delta_erreur*Kd;
        somme_erreur += erreur_theta;
        delta_erreur = erreur_theta - erreur_precedente;
        erreur_precedente = erreur_theta;
        
        m_motorL.setSpeed(-cmd);
        m_motorR.setSpeed(cmd);

        //! Pas bon coeff, mais c'est l'idée
        consigne_g = 0;//-erreur_theta*0.0001;
        consigne_d = 0;//erreur_theta*0.0001;

        /*if(erreur_theta <= abs(0.7))
        {
            done = true;
            logger.printf("Posey\r\n");
            state = 2;
        }*/
    }
    /*switch(etat_angle)
    {
        case 0:

        break;

        case 1:

        break;*/
}