ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp

Committer:
sype
Date:
2015-04-10
Revision:
69:6354497f9f59
Parent:
67:0bdf181586b5
Child:
70:56086a37f31f

File content as of revision 69:6354497f9f59:

#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;
    Kp = 0.01;
}

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