File content as of revision 33:eab29f01e499:

/**
 * @author BERTELONE Benjamin
 *
 * @section DESCRIPTION
 * 
 */

#include "Odometry.h"

Serial pc(USBTX, USBRX); // tx, rx


Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
{
    m_qei_left = qei_left;
    m_qei_right = qei_right;
    m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.14;
    m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.14;
    m_v = v;
    
    m_pulses_left = qei_left->getPulses();
    m_pulses_right = qei_right->getPulses();
    
    setPos(0,0,0);
    setVit(0,0,0);
    dt = time(NULL);
    
    updater.attach(this, &Odometry::update, 0.2);
}

void Odometry::setPos(float x, float y, float theta)
{
    this->x = x;
    this->y = y;
    this->theta = theta;
}

void Odometry::setVit(float Vx, float Vy, float W)
{
    this->Vx = Vx;
    this->Vy = Vy;
    this->W = W;
}

void Odometry::setX(float x)
{
    this->x = x;
}

void Odometry::setY(float Y)
{
    this->y = y;
}

void Odometry::setTheta(float theta)
{
    this->theta = theta;
}

void Odometry::reset()
{
    setPos(0,0,0);
    setVit(0,0,0);
    m_pulses_left = m_qei_left->getPulses();
    m_pulses_right = m_qei_right->getPulses();
}

void Odometry::update()
{
    int delta_left = m_qei_left->getPulses() - m_pulses_left;
    m_pulses_left = m_qei_left->getPulses();
    int delta_right = m_qei_right->getPulses() - m_pulses_right;
    m_pulses_right = m_qei_right->getPulses();
    
    float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
    float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
    
    float dx = deltaS*cos(theta);;
    float dy = deltaS*sin(theta);
    
    x += dx;
    y += dy;
    theta += deltaTheta;
    
    //update velocity
    //dt = time(NULL)-dt;
    dt = 0.2;
    //dt = timer.read_ms()*1e3;
    //pc.printf("%f secondes",dt);
        
    Vx = dx/dt;
    Vy = dy/dt;
    W = deltaTheta/dt;
    
    //timer.stop();
    //timer.reset();
    //timer.start();
}