ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry.cpp@47:4909e97570f6, 2015-03-26 (annotated)

Committer:
Near32
Date:
Thu Mar 26 18:04:23 2015 +0000
Revision:
47:4909e97570f6
Parent:
43:87bdce65341f
Child:
48:cb3ebbc27db3
logger baudrate + usb comm; pid motors; state : phi r and l; asserv vd wd pid phi r and l

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 47:4909e97570f6 32 dt = 0.01;
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
Near32 43:87bdce65341f 38 updater.attach(this, &Odometry::update, dt);
Jagang 33:eab29f01e499 39 }
Jagang 33:eab29f01e499 40
Jagang 33:eab29f01e499 41 void Odometry::setPos(float x, float y, float theta)
Jagang 33:eab29f01e499 42 {
Jagang 33:eab29f01e499 43 this->x = x;
Jagang 33:eab29f01e499 44 this->y = y;
Jagang 33:eab29f01e499 45 this->theta = theta;
Jagang 33:eab29f01e499 46 }
Jagang 33:eab29f01e499 47
Jagang 33:eab29f01e499 48 void Odometry::setVit(float Vx, float Vy, float W)
Jagang 33:eab29f01e499 49 {
Jagang 33:eab29f01e499 50 this->Vx = Vx;
Jagang 33:eab29f01e499 51 this->Vy = Vy;
Jagang 33:eab29f01e499 52 this->W = W;
Jagang 33:eab29f01e499 53 }
Jagang 33:eab29f01e499 54
Near32 47:4909e97570f6 55 void Odometry::setVitPhi(float phi_r, float phi_l)
Near32 47:4909e97570f6 56 {
Near32 47:4909e97570f6 57 this->phi_r = phi_r;
Near32 47:4909e97570f6 58 this->phi_l = phi_l;
Near32 47:4909e97570f6 59 }
Near32 47:4909e97570f6 60
Jagang 33:eab29f01e499 61 void Odometry::setX(float x)
Jagang 33:eab29f01e499 62 {
Jagang 33:eab29f01e499 63 this->x = x;
Jagang 33:eab29f01e499 64 }
Jagang 33:eab29f01e499 65
Jagang 33:eab29f01e499 66 void Odometry::setY(float Y)
Jagang 33:eab29f01e499 67 {
Jagang 33:eab29f01e499 68 this->y = y;
Jagang 33:eab29f01e499 69 }
Jagang 33:eab29f01e499 70
Jagang 33:eab29f01e499 71 void Odometry::setTheta(float theta)
Jagang 33:eab29f01e499 72 {
Jagang 33:eab29f01e499 73 this->theta = theta;
Jagang 33:eab29f01e499 74 }
Jagang 33:eab29f01e499 75
Jagang 33:eab29f01e499 76 void Odometry::reset()
Jagang 33:eab29f01e499 77 {
Jagang 33:eab29f01e499 78 setPos(0,0,0);
Jagang 33:eab29f01e499 79 setVit(0,0,0);
Near32 47:4909e97570f6 80 setVitPhi(0,0);
Jagang 33:eab29f01e499 81 m_pulses_left = m_qei_left->getPulses();
Jagang 33:eab29f01e499 82 m_pulses_right = m_qei_right->getPulses();
Near32 47:4909e97570f6 83 initoffset = false;
Jagang 33:eab29f01e499 84 }
Jagang 33:eab29f01e499 85
Jagang 33:eab29f01e499 86 void Odometry::update()
Jagang 33:eab29f01e499 87 {
Near32 47:4909e97570f6 88 delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 33:eab29f01e499 89 m_pulses_left = m_qei_left->getPulses();
Near32 47:4909e97570f6 90 delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 33:eab29f01e499 91 m_pulses_right = m_qei_right->getPulses();
Jagang 33:eab29f01e499 92
Jagang 33:eab29f01e499 93 float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
Jagang 33:eab29f01e499 94 float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
Jagang 33:eab29f01e499 95
Jagang 33:eab29f01e499 96 float dx = deltaS*cos(theta);;
Jagang 33:eab29f01e499 97 float dy = deltaS*sin(theta);
Jagang 33:eab29f01e499 98
Jagang 33:eab29f01e499 99 x += dx;
Jagang 33:eab29f01e499 100 y += dy;
Jagang 33:eab29f01e499 101 theta += deltaTheta;
Jagang 33:eab29f01e499 102
Jagang 33:eab29f01e499 103 //update velocity
Near32 43:87bdce65341f 104 //dt = time(NULL)-dt;
Jagang 33:eab29f01e499 105 //dt = timer.read_ms()*1e3;
Jagang 33:eab29f01e499 106 //pc.printf("%f secondes",dt);
Jagang 33:eab29f01e499 107
Near32 47:4909e97570f6 108 if(!initoffset)
Near32 47:4909e97570f6 109 {
Near32 47:4909e97570f6 110 offsetVx = dx/dt;
Near32 47:4909e97570f6 111 offsetVy = dy/dt;
Near32 47:4909e97570f6 112 initoffset = true;
Near32 47:4909e97570f6 113 logger.printf("offset Vx = %f \t offset Vy = %f \r\n", offsetVx, offsetVy);
Near32 47:4909e97570f6 114 }
Near32 47:4909e97570f6 115
Near32 47:4909e97570f6 116 Vx = dx/dt-offsetVx;
Near32 47:4909e97570f6 117 Vy = dy/dt-offsetVy;
Near32 47:4909e97570f6 118 W = deltaTheta/dt;
Jagang 33:eab29f01e499 119
Jagang 33:eab29f01e499 120 //timer.stop();
Jagang 33:eab29f01e499 121 //timer.reset();
Jagang 33:eab29f01e499 122 //timer.start();
Jagang 33:eab29f01e499 123 }
Jagang 33:eab29f01e499 124
Jagang 33:eab29f01e499 125
Jagang 33:eab29f01e499 126
Jagang 33:eab29f01e499 127
Jagang 33:eab29f01e499 128