ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry2.cpp@71:95d76c181b22, 2015-04-10 (annotated)

Committer:
Jagang
Date:
Fri Apr 10 18:01:54 2015 +0000
Revision:
71:95d76c181b22
Child:
72:b2a128486332
asservB angle

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 71:95d76c181b22 9
Jagang 71:95d76c181b22 10 extern Serial logger;
Jagang 71:95d76c181b22 11
Jagang 71:95d76c181b22 12
Jagang 71:95d76c181b22 13 Odometry2::Odometry2(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
Jagang 71:95d76c181b22 14 {
Jagang 71:95d76c181b22 15 m_qei_left = qei_left;
Jagang 71:95d76c181b22 16 m_qei_right = qei_right;
Jagang 71:95d76c181b22 17 m_v = v;
Jagang 71:95d76c181b22 18
Jagang 71:95d76c181b22 19 m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.1415;
Jagang 71:95d76c181b22 20 m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.1415;
Jagang 71:95d76c181b22 21
Jagang 71:95d76c181b22 22 m_pulses_left = qei_left->getPulses();
Jagang 71:95d76c181b22 23 m_pulses_right = qei_right->getPulses();
Jagang 71:95d76c181b22 24
Jagang 71:95d76c181b22 25 // Vitesse du moteur gauche et droit
Jagang 71:95d76c181b22 26 m_vitLeft = 0;
Jagang 71:95d76c181b22 27 m_vitRight = 0;
Jagang 71:95d76c181b22 28 }
Jagang 71:95d76c181b22 29
Jagang 71:95d76c181b22 30 void Odometry2::setPos(float x, float y, float theta)
Jagang 71:95d76c181b22 31 {
Jagang 71:95d76c181b22 32 this->x = x;
Jagang 71:95d76c181b22 33 this->y = y;
Jagang 71:95d76c181b22 34 this->theta = theta;
Jagang 71:95d76c181b22 35 }
Jagang 71:95d76c181b22 36
Jagang 71:95d76c181b22 37 void Odometry2::setX(float x)
Jagang 71:95d76c181b22 38 {
Jagang 71:95d76c181b22 39 this->x = x;
Jagang 71:95d76c181b22 40 }
Jagang 71:95d76c181b22 41
Jagang 71:95d76c181b22 42 void Odometry2::setY(float Y)
Jagang 71:95d76c181b22 43 {
Jagang 71:95d76c181b22 44 this->y = y;
Jagang 71:95d76c181b22 45 }
Jagang 71:95d76c181b22 46
Jagang 71:95d76c181b22 47 void Odometry2::setTheta(float theta)
Jagang 71:95d76c181b22 48 {
Jagang 71:95d76c181b22 49 this->theta = theta;
Jagang 71:95d76c181b22 50 }
Jagang 71:95d76c181b22 51
Jagang 71:95d76c181b22 52 void Odometry2::update(float dt)
Jagang 71:95d76c181b22 53 {
Jagang 71:95d76c181b22 54 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 71:95d76c181b22 55 m_pulses_left = m_qei_left->getPulses();
Jagang 71:95d76c181b22 56 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 71:95d76c181b22 57 m_pulses_right = m_qei_right->getPulses();
Jagang 71:95d76c181b22 58
Jagang 71:95d76c181b22 59 m_vitLeft = m_distPerTick_left*delta_left/dt;
Jagang 71:95d76c181b22 60 m_vitRight = m_distPerTick_right*delta_right/dt;
Jagang 71:95d76c181b22 61
Jagang 71:95d76c181b22 62 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
Jagang 71:95d76c181b22 63 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 71:95d76c181b22 64
Jagang 71:95d76c181b22 65 float dx = deltaS*cos(theta);
Jagang 71:95d76c181b22 66 float dy = deltaS*sin(theta);
Jagang 71:95d76c181b22 67
Jagang 71:95d76c181b22 68 x += dx;
Jagang 71:95d76c181b22 69 y += dy;
Jagang 71:95d76c181b22 70 theta += deltaTheta;
Jagang 71:95d76c181b22 71 }
Jagang 71:95d76c181b22 72
Jagang 71:95d76c181b22 73
Jagang 71:95d76c181b22 74
Jagang 71:95d76c181b22 75
Jagang 71:95d76c181b22 76