ARES / Mbed 2 deprecated Robot 2016

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Odometry/Odometry.cpp@0:b127c787a51b, 2015-05-24 (annotated)

Committer:
Jagang
Date:
Sun May 24 12:30:47 2015 +0000
Revision:
0:b127c787a51b
Nettoyage du code d'asserv.; L'asserv ne fonctionne plus, juste test, moteurs ? 20% sur 1m

Who changed what in which revision?

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