ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry2.cpp@85:8e95432d99d3, 2015-04-18 (annotated)

Committer:
sype
Date:
Sat Apr 18 09:39:15 2015 +0000
Revision:
85:8e95432d99d3
Parent:
79:d97090bb6470
Child:
93:4d5664e9188a
Child:
94:5c37bcf73d14
Mise en place de l'asserv de l'ETAT 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
sype 85:8e95432d99d3 26
Jagang 71:95d76c181b22 27 // Vitesse du moteur gauche et droit
Jagang 71:95d76c181b22 28 m_vitLeft = 0;
Jagang 71:95d76c181b22 29 m_vitRight = 0;
Jagang 71:95d76c181b22 30 }
Jagang 71:95d76c181b22 31
Jagang 71:95d76c181b22 32 void Odometry2::setPos(float x, float y, float theta)
Jagang 71:95d76c181b22 33 {
Jagang 71:95d76c181b22 34 this->x = x;
Jagang 71:95d76c181b22 35 this->y = y;
Jagang 71:95d76c181b22 36 this->theta = theta;
Jagang 71:95d76c181b22 37 }
Jagang 71:95d76c181b22 38
Jagang 71:95d76c181b22 39 void Odometry2::setX(float x)
Jagang 71:95d76c181b22 40 {
Jagang 71:95d76c181b22 41 this->x = x;
Jagang 71:95d76c181b22 42 }
Jagang 71:95d76c181b22 43
sype 79:d97090bb6470 44 void Odometry2::setY(float y)
Jagang 71:95d76c181b22 45 {
Jagang 71:95d76c181b22 46 this->y = y;
Jagang 71:95d76c181b22 47 }
Jagang 71:95d76c181b22 48
Jagang 71:95d76c181b22 49 void Odometry2::setTheta(float theta)
Jagang 71:95d76c181b22 50 {
Jagang 71:95d76c181b22 51 this->theta = theta;
Jagang 71:95d76c181b22 52 }
Jagang 71:95d76c181b22 53
Jagang 71:95d76c181b22 54 void Odometry2::update(float dt)
Jagang 71:95d76c181b22 55 {
Jagang 71:95d76c181b22 56 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 71:95d76c181b22 57 m_pulses_left = m_qei_left->getPulses();
Jagang 71:95d76c181b22 58 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 71:95d76c181b22 59 m_pulses_right = m_qei_right->getPulses();
Jagang 71:95d76c181b22 60
sype 85:8e95432d99d3 61 m_distLeft = m_pulses_left*m_distPerTick_left;
sype 85:8e95432d99d3 62 m_distRight = m_pulses_right*m_distPerTick_right;
sype 85:8e95432d99d3 63
Jagang 71:95d76c181b22 64 m_vitLeft = m_distPerTick_left*delta_left/dt;
Jagang 71:95d76c181b22 65 m_vitRight = m_distPerTick_right*delta_right/dt;
Jagang 71:95d76c181b22 66
Jagang 71:95d76c181b22 67 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
Jagang 71:95d76c181b22 68 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 71:95d76c181b22 69
Jagang 71:95d76c181b22 70 float dx = deltaS*cos(theta);
Jagang 71:95d76c181b22 71 float dy = deltaS*sin(theta);
Jagang 71:95d76c181b22 72
Jagang 71:95d76c181b22 73 x += dx;
Jagang 71:95d76c181b22 74 y += dy;
Jagang 71:95d76c181b22 75 theta += deltaTheta;
Jagang 72:b2a128486332 76
Jagang 72:b2a128486332 77 while(theta > PI) theta -= 2*PI;
Jagang 72:b2a128486332 78 while(theta <= -PI) theta += 2*PI;
Jagang 71:95d76c181b22 79 }
Jagang 71:95d76c181b22 80
Jagang 71:95d76c181b22 81
Jagang 71:95d76c181b22 82
Jagang 71:95d76c181b22 83
Jagang 71:95d76c181b22 84