ARES / Mbed 2 deprecated Robot 2015

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@123:55e5e9acc541, 2015-05-11 (annotated)

Committer:
Jagang
Date:
Mon May 11 20:32:11 2015 +0000
Revision:
123:55e5e9acc541
Parent:
89:d05001d85a02 
Maj AI; Repositionnement du d?part (1000,177); Ajout de l'action sur les claps

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 33:eab29f01e499 1 /**
Jagang 33:eab29f01e499 2 * @author BERTELONE Benjamin
Jagang 33:eab29f01e499 3 *
Jagang 33:eab29f01e499 4 * @section DESCRIPTION
Jagang 33:eab29f01e499 5 *
Jagang 33:eab29f01e499 6 */
Jagang 33:eab29f01e499 7
Jagang 33:eab29f01e499 8 #include "Odometry.h"
Jagang 33:eab29f01e499 9
Near32 47:4909e97570f6 10 extern Serial logger;
Jagang 33:eab29f01e499 11
Jagang 33:eab29f01e499 12
Jagang 33:eab29f01e499 13 Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
Jagang 33:eab29f01e499 14 {
Jagang 33:eab29f01e499 15 m_qei_left = qei_left;
Jagang 33:eab29f01e499 16 m_qei_right = qei_right;
Near32 47:4909e97570f6 17 this->radius_left = radius_left;
Near32 47:4909e97570f6 18 this->radius_right = radius_right;
Near32 47:4909e97570f6 19 this->delta_right = 0;
Near32 47:4909e97570f6 20 this->delta_left = 0;
Near32 47:4909e97570f6 21
Near32 89:d05001d85a02 22 m_distPerTick_left = ((float)radius_left)/((float)qei_left->getPulsesPerRev())*2.0f*3.1415f;
Near32 89:d05001d85a02 23 m_distPerTick_right = ((float)radius_right)/((float)qei_right->getPulsesPerRev())*2.0f*3.1415f;
Jagang 33:eab29f01e499 24 m_v = v;
Jagang 33:eab29f01e499 25
Jagang 33:eab29f01e499 26 m_pulses_left = qei_left->getPulses();
Jagang 33:eab29f01e499 27 m_pulses_right = qei_right->getPulses();
Jagang 33:eab29f01e499 28
Jagang 33:eab29f01e499 29 setPos(0,0,0);
Jagang 33:eab29f01e499 30 setVit(0,0,0);
Near32 47:4909e97570f6 31 setVitPhi(0,0);
Near32 48:cb3ebbc27db3 32 dt = 0.05;
Near32 47:4909e97570f6 33
Near32 47:4909e97570f6 34 initoffset = false;
Near32 47:4909e97570f6 35 offsetVx = 0.0;
Near32 47:4909e97570f6 36 offsetVy = 0.0;
Jagang 33:eab29f01e499 37
Jagang 57:ab13f4e7a2b2 38 // Vitesse du moteur gauche et droit
Jagang 57:ab13f4e7a2b2 39 m_vitLeft = 0;
Jagang 57:ab13f4e7a2b2 40 m_vitRight = 0;
Jagang 57:ab13f4e7a2b2 41
Jagang 63:fd9af0693e50 42 //updater.attach(this, &Odometry::update, dt);
Jagang 33:eab29f01e499 43 }
Jagang 33:eab29f01e499 44
Jagang 33:eab29f01e499 45 void Odometry::setPos(float x, float y, float theta)
Jagang 33:eab29f01e499 46 {
Jagang 33:eab29f01e499 47 this->x = x;
Jagang 33:eab29f01e499 48 this->y = y;
Jagang 33:eab29f01e499 49 this->theta = theta;
Jagang 33:eab29f01e499 50 }
Jagang 33:eab29f01e499 51
Jagang 33:eab29f01e499 52 void Odometry::setVit(float Vx, float Vy, float W)
Jagang 33:eab29f01e499 53 {
Jagang 33:eab29f01e499 54 this->Vx = Vx;
Jagang 33:eab29f01e499 55 this->Vy = Vy;
Jagang 33:eab29f01e499 56 this->W = W;
Jagang 33:eab29f01e499 57 }
Jagang 33:eab29f01e499 58
Near32 47:4909e97570f6 59 void Odometry::setVitPhi(float phi_r, float phi_l)
Near32 47:4909e97570f6 60 {
Near32 47:4909e97570f6 61 this->phi_r = phi_r;
Near32 47:4909e97570f6 62 this->phi_l = phi_l;
Near32 47:4909e97570f6 63 }
Near32 47:4909e97570f6 64
Jagang 33:eab29f01e499 65 void Odometry::setX(float x)
Jagang 33:eab29f01e499 66 {
Jagang 33:eab29f01e499 67 this->x = x;
Jagang 33:eab29f01e499 68 }
Jagang 33:eab29f01e499 69
Jagang 33:eab29f01e499 70 void Odometry::setY(float Y)
Jagang 33:eab29f01e499 71 {
Jagang 33:eab29f01e499 72 this->y = y;
Jagang 33:eab29f01e499 73 }
Jagang 33:eab29f01e499 74
Jagang 33:eab29f01e499 75 void Odometry::setTheta(float theta)
Jagang 33:eab29f01e499 76 {
Jagang 33:eab29f01e499 77 this->theta = theta;
Jagang 33:eab29f01e499 78 }
Jagang 33:eab29f01e499 79
Jagang 33:eab29f01e499 80 void Odometry::reset()
Jagang 33:eab29f01e499 81 {
Jagang 33:eab29f01e499 82 setPos(0,0,0);
Jagang 33:eab29f01e499 83 setVit(0,0,0);
Near32 47:4909e97570f6 84 setVitPhi(0,0);
Jagang 33:eab29f01e499 85 m_pulses_left = m_qei_left->getPulses();
Jagang 33:eab29f01e499 86 m_pulses_right = m_qei_right->getPulses();
Near32 47:4909e97570f6 87 initoffset = false;
Jagang 33:eab29f01e499 88 }
Jagang 33:eab29f01e499 89
Jagang 63:fd9af0693e50 90 void Odometry::update(float dt)
Jagang 33:eab29f01e499 91 {
Jagang 71:95d76c181b22 92 this->dt = dt;
Jagang 71:95d76c181b22 93
Near32 47:4909e97570f6 94 delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 33:eab29f01e499 95 m_pulses_left = m_qei_left->getPulses();
Near32 47:4909e97570f6 96 delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 33:eab29f01e499 97 m_pulses_right = m_qei_right->getPulses();
Jagang 33:eab29f01e499 98
Jagang 57:ab13f4e7a2b2 99 m_vitLeft = m_distPerTick_left*delta_left/dt;
Jagang 63:fd9af0693e50 100 m_vitRight = m_distPerTick_right*delta_right/dt;
Jagang 57:ab13f4e7a2b2 101
Near32 89:d05001d85a02 102 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
Jagang 33:eab29f01e499 103 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 33:eab29f01e499 104
Jagang 33:eab29f01e499 105 float dx = deltaS*cos(theta);;
Jagang 33:eab29f01e499 106 float dy = deltaS*sin(theta);
Jagang 33:eab29f01e499 107
Jagang 33:eab29f01e499 108 x += dx;
Jagang 33:eab29f01e499 109 y += dy;
Jagang 33:eab29f01e499 110 theta += deltaTheta;
Jagang 33:eab29f01e499 111
Jagang 33:eab29f01e499 112 //update velocity
Near32 43:87bdce65341f 113 //dt = time(NULL)-dt;
Jagang 33:eab29f01e499 114 //dt = timer.read_ms()*1e3;
Jagang 33:eab29f01e499 115 //pc.printf("%f secondes",dt);
Jagang 33:eab29f01e499 116
Near32 47:4909e97570f6 117 if(!initoffset)
Near32 47:4909e97570f6 118 {
Near32 47:4909e97570f6 119 offsetVx = dx/dt;
Near32 47:4909e97570f6 120 offsetVy = dy/dt;
Near32 47:4909e97570f6 121 initoffset = true;
Near32 48:cb3ebbc27db3 122 //logger.printf("offset Vx = %f \t offset Vy = %f \r\n", offsetVx, offsetVy);
Near32 47:4909e97570f6 123 }
Near32 47:4909e97570f6 124
Near32 47:4909e97570f6 125 Vx = dx/dt-offsetVx;
Near32 47:4909e97570f6 126 Vy = dy/dt-offsetVy;
Near32 48:cb3ebbc27db3 127 W = deltaTheta/dt;
Jagang 33:eab29f01e499 128
Jagang 33:eab29f01e499 129 //timer.stop();
Jagang 33:eab29f01e499 130 //timer.reset();
Jagang 33:eab29f01e499 131 //timer.start();
Jagang 33:eab29f01e499 132 }
Jagang 33:eab29f01e499 133
Jagang 33:eab29f01e499 134
Jagang 33:eab29f01e499 135
Jagang 33:eab29f01e499 136
Jagang 33:eab29f01e499 137