ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry.cpp@57:ab13f4e7a2b2, 2015-04-09 (annotated)

Committer:
Jagang
Date:
Thu Apr 09 16:46:48 2015 +0000
Revision:
57:ab13f4e7a2b2
Parent:
48:cb3ebbc27db3
Child:
63:fd9af0693e50
Vitesse odo

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 47:4909e97570f6 22 m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.1415;
Near32 47:4909e97570f6 23 m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.1415;
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
Near32 43:87bdce65341f 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 33:eab29f01e499 90 void Odometry::update()
Jagang 33:eab29f01e499 91 {
Near32 47:4909e97570f6 92 delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 33:eab29f01e499 93 m_pulses_left = m_qei_left->getPulses();
Near32 47:4909e97570f6 94 delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 33:eab29f01e499 95 m_pulses_right = m_qei_right->getPulses();
Jagang 33:eab29f01e499 96
Jagang 57:ab13f4e7a2b2 97 m_vitLeft = m_distPerTick_left*delta_left/dt;
Jagang 57:ab13f4e7a2b2 98 m_vitLeft = m_distPerTick_right*delta_right/dt;
Jagang 57:ab13f4e7a2b2 99
Jagang 33:eab29f01e499 100 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
Jagang 33:eab29f01e499 101 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 33:eab29f01e499 102
Jagang 33:eab29f01e499 103 float dx = deltaS*cos(theta);;
Jagang 33:eab29f01e499 104 float dy = deltaS*sin(theta);
Jagang 33:eab29f01e499 105
Jagang 33:eab29f01e499 106 x += dx;
Jagang 33:eab29f01e499 107 y += dy;
Jagang 33:eab29f01e499 108 theta += deltaTheta;
Jagang 33:eab29f01e499 109
Jagang 33:eab29f01e499 110 //update velocity
Near32 43:87bdce65341f 111 //dt = time(NULL)-dt;
Jagang 33:eab29f01e499 112 //dt = timer.read_ms()*1e3;
Jagang 33:eab29f01e499 113 //pc.printf("%f secondes",dt);
Jagang 33:eab29f01e499 114
Near32 47:4909e97570f6 115 if(!initoffset)
Near32 47:4909e97570f6 116 {
Near32 47:4909e97570f6 117 offsetVx = dx/dt;
Near32 47:4909e97570f6 118 offsetVy = dy/dt;
Near32 47:4909e97570f6 119 initoffset = true;
Near32 48:cb3ebbc27db3 120 //logger.printf("offset Vx = %f \t offset Vy = %f \r\n", offsetVx, offsetVy);
Near32 47:4909e97570f6 121 }
Near32 47:4909e97570f6 122
Near32 47:4909e97570f6 123 Vx = dx/dt-offsetVx;
Near32 47:4909e97570f6 124 Vy = dy/dt-offsetVy;
Near32 48:cb3ebbc27db3 125 W = deltaTheta/dt;
Jagang 33:eab29f01e499 126
Jagang 33:eab29f01e499 127 //timer.stop();
Jagang 33:eab29f01e499 128 //timer.reset();
Jagang 33:eab29f01e499 129 //timer.start();
Jagang 33:eab29f01e499 130 }
Jagang 33:eab29f01e499 131
Jagang 33:eab29f01e499 132
Jagang 33:eab29f01e499 133
Jagang 33:eab29f01e499 134
Jagang 33:eab29f01e499 135