ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry.cpp@30:33e970ba1fe5, 2015-02-01 (annotated)

Committer:
Near32
Date:
Sun Feb 01 19:29:14 2015 +0000
Revision:
30:33e970ba1fe5
Parent:
0:41149573d577 
Kalman Filter : Noise handling with Normal Distribution (for measurement only).; Odometry : velocities.; Test runned : ; - odometry checking : problem : drift... (changed the frequency of the ticker, doesn't seem to fix the problem ?); -

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);
Near32 30:33e970ba1fe5 25 setVit(0,0,0);
Near32 30:33e970ba1fe5 26 dt = time(NULL);
Jagang 0:41149573d577 27
Near32 30:33e970ba1fe5 28 updater.attach(this, &Odometry::update, 0.2);
Jagang 0:41149573d577 29 }
Jagang 0:41149573d577 30
Jagang 0:41149573d577 31 void Odometry::setPos(float x, float y, float theta)
Jagang 0:41149573d577 32 {
Jagang 0:41149573d577 33 this->x = x;
Jagang 0:41149573d577 34 this->y = y;
Jagang 0:41149573d577 35 this->theta = theta;
Jagang 0:41149573d577 36 }
Jagang 0:41149573d577 37
Near32 30:33e970ba1fe5 38 void Odometry::setVit(float Vx, float Vy, float W)
Near32 30:33e970ba1fe5 39 {
Near32 30:33e970ba1fe5 40 this->Vx = Vx;
Near32 30:33e970ba1fe5 41 this->Vy = Vy;
Near32 30:33e970ba1fe5 42 this->W = W;
Near32 30:33e970ba1fe5 43 }
Near32 30:33e970ba1fe5 44
Jagang 0:41149573d577 45 void Odometry::setX(float x)
Jagang 0:41149573d577 46 {
Jagang 0:41149573d577 47 this->x = x;
Jagang 0:41149573d577 48 }
Jagang 0:41149573d577 49
Jagang 0:41149573d577 50 void Odometry::setY(float Y)
Jagang 0:41149573d577 51 {
Jagang 0:41149573d577 52 this->y = y;
Jagang 0:41149573d577 53 }
Jagang 0:41149573d577 54
Jagang 0:41149573d577 55 void Odometry::setTheta(float theta)
Jagang 0:41149573d577 56 {
Jagang 0:41149573d577 57 this->theta = theta;
Jagang 0:41149573d577 58 }
Jagang 0:41149573d577 59
Jagang 0:41149573d577 60 void Odometry::reset()
Jagang 0:41149573d577 61 {
Jagang 0:41149573d577 62 setPos(0,0,0);
Near32 30:33e970ba1fe5 63 setVit(0,0,0);
Jagang 0:41149573d577 64 m_pulses_left = m_qei_left->getPulses();
Jagang 0:41149573d577 65 m_pulses_right = m_qei_right->getPulses();
Jagang 0:41149573d577 66 }
Jagang 0:41149573d577 67
Jagang 0:41149573d577 68 void Odometry::update()
Jagang 0:41149573d577 69 {
Jagang 0:41149573d577 70 DigitalOut myled(LED1);
Near32 30:33e970ba1fe5 71 myled = 1;
Near32 30:33e970ba1fe5 72
Jagang 0:41149573d577 73 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 0:41149573d577 74 m_pulses_left = m_qei_left->getPulses();
Jagang 0:41149573d577 75 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 0:41149573d577 76 m_pulses_right = m_qei_right->getPulses();
Jagang 0:41149573d577 77
Jagang 0:41149573d577 78 float deltaS = (m_radiusPerTick_left*delta_left + m_radiusPerTick_right*delta_right) / 2.0;
Jagang 0:41149573d577 79 float deltaTheta = (m_radiusPerTick_right*delta_right - m_radiusPerTick_left*delta_left) / m_v;
Jagang 0:41149573d577 80
Near32 30:33e970ba1fe5 81 float dx = deltaS*cos(theta);;
Near32 30:33e970ba1fe5 82 float dy = deltaS*sin(theta);
Near32 30:33e970ba1fe5 83
Near32 30:33e970ba1fe5 84 x += dx;
Near32 30:33e970ba1fe5 85 y += dy;
Jagang 0:41149573d577 86 theta += deltaTheta;
Near32 30:33e970ba1fe5 87
Near32 30:33e970ba1fe5 88 //update velocity
Near32 30:33e970ba1fe5 89 //dt = time(NULL)-dt;
Near32 30:33e970ba1fe5 90 dt = 0.2;
Near32 30:33e970ba1fe5 91 //dt = timer.read_ms()*1e3;
Near32 30:33e970ba1fe5 92 //pc.printf("%f secondes",dt);
Near32 30:33e970ba1fe5 93
Near32 30:33e970ba1fe5 94 Vx = dx/dt;
Near32 30:33e970ba1fe5 95 Vy = dy/dt;
Near32 30:33e970ba1fe5 96 W = deltaTheta/dt;
Near32 30:33e970ba1fe5 97
Near32 30:33e970ba1fe5 98 //timer.stop();
Near32 30:33e970ba1fe5 99 //timer.reset();
Near32 30:33e970ba1fe5 100 //timer.start();
Near32 30:33e970ba1fe5 101
Near32 30:33e970ba1fe5 102 myled = 0;
Jagang 0:41149573d577 103 }
Jagang 0:41149573d577 104
Jagang 0:41149573d577 105
Jagang 0:41149573d577 106
Jagang 0:41149573d577 107
Jagang 0:41149573d577 108