ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry.cpp@33:eab29f01e499, 2015-02-03 (annotated)

Committer:
Jagang
Date:
Tue Feb 03 18:47:02 2015 +0000
Revision:
33:eab29f01e499
Child:
43:87bdce65341f
Correction d'odometrie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 33:eab29f01e499 1 /**
Jagang 33:eab29f01e499 2 * @author BERTELONE Benjamin
Jagang 33:eab29f01e499 3 *
Jagang 33:eab29f01e499 4 * @section DESCRIPTION
Jagang 33:eab29f01e499 5 *
Jagang 33:eab29f01e499 6 */
Jagang 33:eab29f01e499 7
Jagang 33:eab29f01e499 8 #include "Odometry.h"
Jagang 33:eab29f01e499 9
Jagang 33:eab29f01e499 10 Serial pc(USBTX, USBRX); // tx, rx
Jagang 33:eab29f01e499 11
Jagang 33:eab29f01e499 12
Jagang 33:eab29f01e499 13 Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
Jagang 33:eab29f01e499 14 {
Jagang 33:eab29f01e499 15 m_qei_left = qei_left;
Jagang 33:eab29f01e499 16 m_qei_right = qei_right;
Jagang 33:eab29f01e499 17 m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.14;
Jagang 33:eab29f01e499 18 m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.14;
Jagang 33:eab29f01e499 19 m_v = v;
Jagang 33:eab29f01e499 20
Jagang 33:eab29f01e499 21 m_pulses_left = qei_left->getPulses();
Jagang 33:eab29f01e499 22 m_pulses_right = qei_right->getPulses();
Jagang 33:eab29f01e499 23
Jagang 33:eab29f01e499 24 setPos(0,0,0);
Jagang 33:eab29f01e499 25 setVit(0,0,0);
Jagang 33:eab29f01e499 26 dt = time(NULL);
Jagang 33:eab29f01e499 27
Jagang 33:eab29f01e499 28 updater.attach(this, &Odometry::update, 0.2);
Jagang 33:eab29f01e499 29 }
Jagang 33:eab29f01e499 30
Jagang 33:eab29f01e499 31 void Odometry::setPos(float x, float y, float theta)
Jagang 33:eab29f01e499 32 {
Jagang 33:eab29f01e499 33 this->x = x;
Jagang 33:eab29f01e499 34 this->y = y;
Jagang 33:eab29f01e499 35 this->theta = theta;
Jagang 33:eab29f01e499 36 }
Jagang 33:eab29f01e499 37
Jagang 33:eab29f01e499 38 void Odometry::setVit(float Vx, float Vy, float W)
Jagang 33:eab29f01e499 39 {
Jagang 33:eab29f01e499 40 this->Vx = Vx;
Jagang 33:eab29f01e499 41 this->Vy = Vy;
Jagang 33:eab29f01e499 42 this->W = W;
Jagang 33:eab29f01e499 43 }
Jagang 33:eab29f01e499 44
Jagang 33:eab29f01e499 45 void Odometry::setX(float x)
Jagang 33:eab29f01e499 46 {
Jagang 33:eab29f01e499 47 this->x = x;
Jagang 33:eab29f01e499 48 }
Jagang 33:eab29f01e499 49
Jagang 33:eab29f01e499 50 void Odometry::setY(float Y)
Jagang 33:eab29f01e499 51 {
Jagang 33:eab29f01e499 52 this->y = y;
Jagang 33:eab29f01e499 53 }
Jagang 33:eab29f01e499 54
Jagang 33:eab29f01e499 55 void Odometry::setTheta(float theta)
Jagang 33:eab29f01e499 56 {
Jagang 33:eab29f01e499 57 this->theta = theta;
Jagang 33:eab29f01e499 58 }
Jagang 33:eab29f01e499 59
Jagang 33:eab29f01e499 60 void Odometry::reset()
Jagang 33:eab29f01e499 61 {
Jagang 33:eab29f01e499 62 setPos(0,0,0);
Jagang 33:eab29f01e499 63 setVit(0,0,0);
Jagang 33:eab29f01e499 64 m_pulses_left = m_qei_left->getPulses();
Jagang 33:eab29f01e499 65 m_pulses_right = m_qei_right->getPulses();
Jagang 33:eab29f01e499 66 }
Jagang 33:eab29f01e499 67
Jagang 33:eab29f01e499 68 void Odometry::update()
Jagang 33:eab29f01e499 69 {
Jagang 33:eab29f01e499 70 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 33:eab29f01e499 71 m_pulses_left = m_qei_left->getPulses();
Jagang 33:eab29f01e499 72 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 33:eab29f01e499 73 m_pulses_right = m_qei_right->getPulses();
Jagang 33:eab29f01e499 74
Jagang 33:eab29f01e499 75 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
Jagang 33:eab29f01e499 76 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 33:eab29f01e499 77
Jagang 33:eab29f01e499 78 float dx = deltaS*cos(theta);;
Jagang 33:eab29f01e499 79 float dy = deltaS*sin(theta);
Jagang 33:eab29f01e499 80
Jagang 33:eab29f01e499 81 x += dx;
Jagang 33:eab29f01e499 82 y += dy;
Jagang 33:eab29f01e499 83 theta += deltaTheta;
Jagang 33:eab29f01e499 84
Jagang 33:eab29f01e499 85 //update velocity
Jagang 33:eab29f01e499 86 //dt = time(NULL)-dt;
Jagang 33:eab29f01e499 87 dt = 0.2;
Jagang 33:eab29f01e499 88 //dt = timer.read_ms()*1e3;
Jagang 33:eab29f01e499 89 //pc.printf("%f secondes",dt);
Jagang 33:eab29f01e499 90
Jagang 33:eab29f01e499 91 Vx = dx/dt;
Jagang 33:eab29f01e499 92 Vy = dy/dt;
Jagang 33:eab29f01e499 93 W = deltaTheta/dt;
Jagang 33:eab29f01e499 94
Jagang 33:eab29f01e499 95 //timer.stop();
Jagang 33:eab29f01e499 96 //timer.reset();
Jagang 33:eab29f01e499 97 //timer.start();
Jagang 33:eab29f01e499 98 }
Jagang 33:eab29f01e499 99
Jagang 33:eab29f01e499 100
Jagang 33:eab29f01e499 101
Jagang 33:eab29f01e499 102
Jagang 33:eab29f01e499 103