ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Odometry/Odometry.cpp@1:c8dc3dee3b70, 2014-09-28 (annotated)

Committer:
Jagang
Date:
Sun Sep 28 20:02:02 2014 +0000
Revision:
1:c8dc3dee3b70
Parent:
0:41149573d577 
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "Odometry.h"
Jagang 0:41149573d577 2
Jagang 0:41149573d577 3 Serial pc(USBTX, USBRX); // tx, rx
Jagang 0:41149573d577 4
Jagang 0:41149573d577 5
Jagang 0:41149573d577 6 Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
Jagang 0:41149573d577 7 {
Jagang 0:41149573d577 8 m_qei_left = qei_left;
Jagang 0:41149573d577 9 m_qei_right = qei_right;
Jagang 0:41149573d577 10 m_radiusPerTick_left = radius_left/qei_left->getPulsesPerRev();
Jagang 0:41149573d577 11 m_radiusPerTick_right = radius_right/qei_right->getPulsesPerRev();
Jagang 0:41149573d577 12 m_v = v;
Jagang 0:41149573d577 13
Jagang 0:41149573d577 14 m_pulses_left = qei_left->getPulses();
Jagang 0:41149573d577 15 m_pulses_right = qei_right->getPulses();
Jagang 0:41149573d577 16
Jagang 0:41149573d577 17 setPos(0,0,0);
Jagang 0:41149573d577 18
Jagang 0:41149573d577 19 updater.attach(this, &Odometry::update, 0.5);
Jagang 0:41149573d577 20 }
Jagang 0:41149573d577 21
Jagang 0:41149573d577 22 void Odometry::setPos(float x, float y, float theta)
Jagang 0:41149573d577 23 {
Jagang 0:41149573d577 24 this->x = x;
Jagang 0:41149573d577 25 this->y = y;
Jagang 0:41149573d577 26 this->theta = theta;
Jagang 0:41149573d577 27 }
Jagang 0:41149573d577 28
Jagang 0:41149573d577 29 void Odometry::setX(float x)
Jagang 0:41149573d577 30 {
Jagang 0:41149573d577 31 this->x = x;
Jagang 0:41149573d577 32 }
Jagang 0:41149573d577 33
Jagang 0:41149573d577 34 void Odometry::setY(float Y)
Jagang 0:41149573d577 35 {
Jagang 0:41149573d577 36 this->y = y;
Jagang 0:41149573d577 37 }
Jagang 0:41149573d577 38
Jagang 0:41149573d577 39 void Odometry::setTheta(float theta)
Jagang 0:41149573d577 40 {
Jagang 0:41149573d577 41 this->theta = theta;
Jagang 0:41149573d577 42 }
Jagang 0:41149573d577 43
Jagang 0:41149573d577 44 void Odometry::reset()
Jagang 0:41149573d577 45 {
Jagang 0:41149573d577 46 setPos(0,0,0);
Jagang 0:41149573d577 47 m_pulses_left = m_qei_left->getPulses();
Jagang 0:41149573d577 48 m_pulses_right = m_qei_right->getPulses();
Jagang 0:41149573d577 49 }
Jagang 0:41149573d577 50
Jagang 0:41149573d577 51 void Odometry::update()
Jagang 0:41149573d577 52 {
Jagang 0:41149573d577 53 DigitalOut myled(LED1);
Jagang 0:41149573d577 54 myled = 1;
Jagang 0:41149573d577 55 int delta_left = m_qei_left->getPulses() - m_pulses_left;
Jagang 0:41149573d577 56 m_pulses_left = m_qei_left->getPulses();
Jagang 0:41149573d577 57 int delta_right = m_qei_right->getPulses() - m_pulses_right;
Jagang 0:41149573d577 58 m_pulses_right = m_qei_right->getPulses();
Jagang 0:41149573d577 59
Jagang 0:41149573d577 60 float deltaS = (m_radiusPerTick_left*delta_left + m_radiusPerTick_right*delta_right) / 2.0;
Jagang 0:41149573d577 61 float deltaTheta = (m_radiusPerTick_right*delta_right - m_radiusPerTick_left*delta_left) / m_v;
Jagang 0:41149573d577 62
Jagang 0:41149573d577 63 x += deltaS*cos(theta);
Jagang 0:41149573d577 64 y += deltaS*sin(theta);
Jagang 0:41149573d577 65 theta += deltaTheta;
Jagang 0:41149573d577 66 myled = 0;
Jagang 0:41149573d577 67 }
Jagang 0:41149573d577 68
Jagang 0:41149573d577 69
Jagang 0:41149573d577 70
Jagang 0:41149573d577 71
Jagang 0:41149573d577 72