Diff: Odometry/Odometry.cpp

Revision:

47:4909e97570f6

Parent:

43:87bdce65341f

Child:

48:cb3ebbc27db3

--- a/Odometry/Odometry.cpp	Tue Mar 24 09:17:11 2015 +0000
+++ b/Odometry/Odometry.cpp	Thu Mar 26 18:04:23 2015 +0000
@@ -7,15 +7,20 @@
 
 #include "Odometry.h"
 
-Serial pc(USBTX, USBRX); // tx, rx
+extern Serial logger;
 
 
 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;
+    this->radius_left = radius_left;
+    this->radius_right = radius_right;
+    this->delta_right = 0;
+    this->delta_left = 0;
+    
+    m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*3.1415;
+    m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*3.1415;
     m_v = v;
     
     m_pulses_left = qei_left->getPulses();
@@ -23,7 +28,12 @@
     
     setPos(0,0,0);
     setVit(0,0,0);
-    dt = 0.1;
+    setVitPhi(0,0);
+    dt = 0.01;
+    
+    initoffset = false;
+    offsetVx = 0.0;
+    offsetVy = 0.0;
     
     updater.attach(this, &Odometry::update, dt);
 }
@@ -42,6 +52,12 @@
     this->W = W;
 }
 
+void Odometry::setVitPhi(float phi_r, float phi_l)
+{
+    this->phi_r = phi_r;
+    this->phi_l = phi_l;
+}    
+
 void Odometry::setX(float x)
 {
     this->x = x;
@@ -61,15 +77,17 @@
 {
     setPos(0,0,0);
     setVit(0,0,0);
+    setVitPhi(0,0);
     m_pulses_left = m_qei_left->getPulses();
     m_pulses_right = m_qei_right->getPulses();
+    initoffset = false;
 }
 
 void Odometry::update()
 {
-    int delta_left = m_qei_left->getPulses() - m_pulses_left;
+    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;
+    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;
@@ -87,9 +105,17 @@
     //dt = timer.read_ms()*1e3;
     //pc.printf("%f secondes",dt);
         
-    Vx = dx/dt;
-    Vy = dy/dt;
-    W = deltaTheta/dt;
+    if(!initoffset)
+    {
+        offsetVx = dx/dt;
+        offsetVy = dy/dt;
+        initoffset = true;
+        logger.printf("offset Vx = %f \t offset Vy = %f \r\n", offsetVx, offsetVy);
+    }
+    
+    Vx = dx/dt-offsetVx;
+    Vy = dy/dt-offsetVy;
+    W = deltaTheta/dt;        
     
     //timer.stop();
     //timer.reset();