Diff: Odometry/Odometry.cpp

Revision:

33:eab29f01e499

Child:

43:87bdce65341f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/Odometry.cpp	Tue Feb 03 18:47:02 2015 +0000
@@ -0,0 +1,103 @@
+/**
+ * @author BERTELONE Benjamin
+ *
+ * @section DESCRIPTION
+ * 
+ */
+
+#include "Odometry.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+
+Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
+{
+    m_qei_left = qei_left;
+    m_qei_right = qei_right;
+    m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.14;
+    m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.14;
+    m_v = v;
+    
+    m_pulses_left = qei_left->getPulses();
+    m_pulses_right = qei_right->getPulses();
+    
+    setPos(0,0,0);
+    setVit(0,0,0);
+    dt = time(NULL);
+    
+    updater.attach(this, &Odometry::update, 0.2);
+}
+
+void Odometry::setPos(float x, float y, float theta)
+{
+    this->x = x;
+    this->y = y;
+    this->theta = theta;
+}
+
+void Odometry::setVit(float Vx, float Vy, float W)
+{
+    this->Vx = Vx;
+    this->Vy = Vy;
+    this->W = W;
+}
+
+void Odometry::setX(float x)
+{
+    this->x = x;
+}
+
+void Odometry::setY(float Y)
+{
+    this->y = y;
+}
+
+void Odometry::setTheta(float theta)
+{
+    this->theta = theta;
+}
+
+void Odometry::reset()
+{
+    setPos(0,0,0);
+    setVit(0,0,0);
+    m_pulses_left = m_qei_left->getPulses();
+    m_pulses_right = m_qei_right->getPulses();
+}
+
+void Odometry::update()
+{
+    int delta_left = m_qei_left->getPulses() - m_pulses_left;
+    m_pulses_left = m_qei_left->getPulses();
+    int delta_right = m_qei_right->getPulses() - m_pulses_right;
+    m_pulses_right = m_qei_right->getPulses();
+    
+    float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0;
+    float deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
+    
+    float dx = deltaS*cos(theta);;
+    float dy = deltaS*sin(theta);
+    
+    x += dx;
+    y += dy;
+    theta += deltaTheta;
+    
+    //update velocity
+    //dt = time(NULL)-dt;
+    dt = 0.2;
+    //dt = timer.read_ms()*1e3;
+    //pc.printf("%f secondes",dt);
+        
+    Vx = dx/dt;
+    Vy = dy/dt;
+    W = deltaTheta/dt;
+    
+    //timer.stop();
+    //timer.reset();
+    //timer.start();
+}
+
+
+
+   
+    
\ No newline at end of file