ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry2.cpp@100:a827a645d6c2, 2015-04-30 (annotated)

Committer:
Jagang
Date:
Thu Apr 30 16:16:29 2015 +0000
Revision:
100:a827a645d6c2
Parent:
94:5c37bcf73d14
Parent:
93:4d5664e9188a 
Merge 2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 71:95d76c181b22 1 /**
Jagang 71:95d76c181b22 2 * @author BERTELONE Benjamin
Jagang 71:95d76c181b22 3 *
Jagang 71:95d76c181b22 4 * @section DESCRIPTION
Jagang 71:95d76c181b22 5 *
Jagang 71:95d76c181b22 6 */
Jagang 71:95d76c181b22 7
Jagang 71:95d76c181b22 8 #include "Odometry2.h"
Jagang 72:b2a128486332 9 #include "defines.h"
Jagang 71:95d76c181b22 10
Jagang 71:95d76c181b22 11 extern Serial logger;
Jagang 71:95d76c181b22 12
Jagang 71:95d76c181b22 13
Jagang 71:95d76c181b22 14 Odometry2::Odometry2(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
Jagang 71:95d76c181b22 15 {
Jagang 71:95d76c181b22 16 m_qei_left = qei_left;
Jagang 71:95d76c181b22 17 m_qei_right = qei_right;
Jagang 71:95d76c181b22 18 m_v = v;
Jagang 71:95d76c181b22 19
Jagang 72:b2a128486332 20 m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*PI;
Jagang 72:b2a128486332 21 m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*PI;
Jagang 71:95d76c181b22 22
Jagang 71:95d76c181b22 23 m_pulses_left = qei_left->getPulses();
Jagang 71:95d76c181b22 24 m_pulses_right = qei_right->getPulses();
Jagang 71:95d76c181b22 25
Jagang 71:95d76c181b22 26 // Vitesse du moteur gauche et droit
Jagang 71:95d76c181b22 27 m_vitLeft = 0;
Jagang 71:95d76c181b22 28 m_vitRight = 0;
Jagang 71:95d76c181b22 29 }
Jagang 71:95d76c181b22 30
Jagang 71:95d76c181b22 31 void Odometry2::setPos(float x, float y, float theta)
Jagang 71:95d76c181b22 32 {
Jagang 71:95d76c181b22 33 this->x = x;
Jagang 71:95d76c181b22 34 this->y = y;
Jagang 71:95d76c181b22 35 this->theta = theta;
Jagang 71:95d76c181b22 36 }
Jagang 71:95d76c181b22 37
Jagang 71:95d76c181b22 38 void Odometry2::setX(float x)
Jagang 71:95d76c181b22 39 {
Jagang 71:95d76c181b22 40 this->x = x;
Jagang 71:95d76c181b22 41 }
Jagang 71:95d76c181b22 42
sype 79:d97090bb6470 43 void Odometry2::setY(float y)
Jagang 71:95d76c181b22 44 {
Jagang 71:95d76c181b22 45 this->y = y;
Jagang 71:95d76c181b22 46 }
Jagang 71:95d76c181b22 47
Jagang 71:95d76c181b22 48 void Odometry2::setTheta(float theta)
Jagang 71:95d76c181b22 49 {
Jagang 71:95d76c181b22 50 this->theta = theta;
Jagang 71:95d76c181b22 51 }
Jagang 71:95d76c181b22 52
Jagang 71:95d76c181b22 53 void Odometry2::update(float dt)
Jagang 71:95d76c181b22 54 {
Jagang 71:95d76c181b22 55 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 71:95d76c181b22 56 m_pulses_left = m_qei_left->getPulses();
Jagang 71:95d76c181b22 57 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 71:95d76c181b22 58 m_pulses_right = m_qei_right->getPulses();
Jagang 71:95d76c181b22 59
Jagang 71:95d76c181b22 60 m_vitLeft = m_distPerTick_left*delta_left/dt;
Jagang 71:95d76c181b22 61 m_vitRight = m_distPerTick_right*delta_right/dt;
Jagang 71:95d76c181b22 62
Jagang 93:4d5664e9188a 63 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
Jagang 71:95d76c181b22 64 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 71:95d76c181b22 65
Jagang 71:95d76c181b22 66 float dx = deltaS*cos(theta);
Jagang 71:95d76c181b22 67 float dy = deltaS*sin(theta);
Jagang 71:95d76c181b22 68
Jagang 71:95d76c181b22 69 x += dx;
Jagang 71:95d76c181b22 70 y += dy;
Jagang 71:95d76c181b22 71 theta += deltaTheta;
Jagang 72:b2a128486332 72
Jagang 72:b2a128486332 73 while(theta > PI) theta -= 2*PI;
Jagang 72:b2a128486332 74 while(theta <= -PI) theta += 2*PI;
Jagang 71:95d76c181b22 75 }
Jagang 71:95d76c181b22 76
Jagang 71:95d76c181b22 77