File content as of revision 70:56086a37f31f:

#include "planB.h"

aserv_planB::aserv_planB(Odometry &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
{
    consigne_g = 10.0;
    consigne_d = 10.0;
    vitesse_g = 0;
    vitesse_d = 0;
    erreur_g = 0;
    erreur_d = 0;
    cmd_g = 0;
    cmd_d = 0;
    somme_erreur_g = 0;
    somme_erreur_d = 0;
    Kp = 0.02;
}

void aserv_planB::control_speed()
{
    vitesse_g = m_odometry.getVitRight();
    vitesse_d = 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)
{
    control_speed();
}

void aserv_planB::control_position()
{
    position_g = m_odometry.getPosRight();
    position_d = m_odometry.getPosLeft();
    
    erreur_position_g = consigne_position_g - position_g;
    cmd_g = erreur_position_g*Kp;
    
    erreur_position_d = consigne_position_d - position_d;
    cmd_d = erreur_position_d*Kp;
    
    m_motorL.setSpeed(cmd_g);
    m_motorR.setSpeed(cmd_d);
}