ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp

Committer:
sype
Date:
2015-04-09
Revision:
64:6489bcfc1173
Parent:
62:454cd844fe1e
Child:
67:0bdf181586b5

File content as of revision 64:6489bcfc1173:

#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.05;
}

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