ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry.cpp@25:c5773b021bb0, 2015-01-20 (annotated)

Committer:
Jagang
Date:
Tue Jan 20 14:34:23 2015 +0000
Revision:
25:c5773b021bb0
Parent:
0:41149573d577 
Adding odometry

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 /**
Jagang 0:41149573d577 2 * @author BERTELONE Benjamin
Jagang 0:41149573d577 3 *
Jagang 0:41149573d577 4 * @section DESCRIPTION
Jagang 0:41149573d577 5 *
Jagang 0:41149573d577 6 */
Jagang 0:41149573d577 7
Jagang 0:41149573d577 8 #include "Odometry.h"
Jagang 0:41149573d577 9
Jagang 0:41149573d577 10 Serial pc(USBTX, USBRX); // tx, rx
Jagang 0:41149573d577 11
Jagang 0:41149573d577 12
Jagang 0:41149573d577 13 Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
Jagang 0:41149573d577 14 {
Jagang 0:41149573d577 15 m_qei_left = qei_left;
Jagang 0:41149573d577 16 m_qei_right = qei_right;
Jagang 0:41149573d577 17 m_radiusPerTick_left = radius_left/qei_left->getPulsesPerRev();
Jagang 0:41149573d577 18 m_radiusPerTick_right = radius_right/qei_right->getPulsesPerRev();
Jagang 0:41149573d577 19 m_v = v;
Jagang 0:41149573d577 20
Jagang 0:41149573d577 21 m_pulses_left = qei_left->getPulses();
Jagang 0:41149573d577 22 m_pulses_right = qei_right->getPulses();
Jagang 0:41149573d577 23
Jagang 0:41149573d577 24 setPos(0,0,0);
Jagang 0:41149573d577 25
Jagang 0:41149573d577 26 updater.attach(this, &Odometry::update, 0.5);
Jagang 0:41149573d577 27 }
Jagang 0:41149573d577 28
Jagang 0:41149573d577 29 void Odometry::setPos(float x, float y, float theta)
Jagang 0:41149573d577 30 {
Jagang 0:41149573d577 31 this->x = x;
Jagang 0:41149573d577 32 this->y = y;
Jagang 0:41149573d577 33 this->theta = theta;
Jagang 0:41149573d577 34 }
Jagang 0:41149573d577 35
Jagang 0:41149573d577 36 void Odometry::setX(float x)
Jagang 0:41149573d577 37 {
Jagang 0:41149573d577 38 this->x = x;
Jagang 0:41149573d577 39 }
Jagang 0:41149573d577 40
Jagang 0:41149573d577 41 void Odometry::setY(float Y)
Jagang 0:41149573d577 42 {
Jagang 0:41149573d577 43 this->y = y;
Jagang 0:41149573d577 44 }
Jagang 0:41149573d577 45
Jagang 0:41149573d577 46 void Odometry::setTheta(float theta)
Jagang 0:41149573d577 47 {
Jagang 0:41149573d577 48 this->theta = theta;
Jagang 0:41149573d577 49 }
Jagang 0:41149573d577 50
Jagang 0:41149573d577 51 void Odometry::reset()
Jagang 0:41149573d577 52 {
Jagang 0:41149573d577 53 setPos(0,0,0);
Jagang 0:41149573d577 54 m_pulses_left = m_qei_left->getPulses();
Jagang 0:41149573d577 55 m_pulses_right = m_qei_right->getPulses();
Jagang 0:41149573d577 56 }
Jagang 0:41149573d577 57
Jagang 0:41149573d577 58 void Odometry::update()
Jagang 0:41149573d577 59 {
Jagang 0:41149573d577 60 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 0:41149573d577 61 m_pulses_left = m_qei_left->getPulses();
Jagang 0:41149573d577 62 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 0:41149573d577 63 m_pulses_right = m_qei_right->getPulses();
Jagang 0:41149573d577 64
Jagang 0:41149573d577 65 float deltaS = (m_radiusPerTick_left*delta_left + m_radiusPerTick_right*delta_right) / 2.0;
Jagang 0:41149573d577 66 float deltaTheta = (m_radiusPerTick_right*delta_right - m_radiusPerTick_left*delta_left) / m_v;
Jagang 0:41149573d577 67
Jagang 0:41149573d577 68 x += deltaS*cos(theta);
Jagang 0:41149573d577 69 y += deltaS*sin(theta);
Jagang 0:41149573d577 70 theta += deltaTheta;
Jagang 0:41149573d577 71 }
Jagang 0:41149573d577 72
Jagang 0:41149573d577 73
Jagang 0:41149573d577 74
Jagang 0:41149573d577 75
Jagang 0:41149573d577 76