Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Odometry/Odometry.cpp
- Committer:
- Jagang
- Date:
- 2015-05-24
- Revision:
- 0:b127c787a51b
File content as of revision 0:b127c787a51b:
/** * @author BERTELONE Benjamin * * @section DESCRIPTION * */ #include "Odometry.h" #include "defines.h" extern Serial logger; 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_v = v; m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*PI; m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*PI; m_pulses_left = qei_left->getPulses(); m_pulses_right = qei_right->getPulses(); // Vitesse du moteur gauche et droit m_vitLeft = 0; m_vitRight = 0; } void Odometry::setPos(float x, float y, float theta) { this->x = x; this->y = y; this->theta = theta; } 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::update(float dt) { 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(); m_distLeft = m_pulses_left*m_distPerTick_left; m_distRight = m_pulses_right*m_distPerTick_right; m_vitLeft = m_distPerTick_left*delta_left/dt; m_vitRight = m_distPerTick_right*delta_right/dt; float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f; float deltaTheta = atan((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; while(theta > PI) theta -= 2*PI; while(theta <= -PI) theta += 2*PI; }