Diff: Odometry/Odometry.cpp

Revision:

30:33e970ba1fe5

Parent:

0:41149573d577

--- a/Odometry/Odometry.cpp	Mon Jan 26 22:40:37 2015 +0000
+++ b/Odometry/Odometry.cpp	Sun Feb 01 19:29:14 2015 +0000
@@ -22,8 +22,10 @@
     m_pulses_right = qei_right->getPulses();
     
     setPos(0,0,0);
+    setVit(0,0,0);
+    dt = time(NULL);
     
-    updater.attach(this, &Odometry::update, 0.5);
+    updater.attach(this, &Odometry::update, 0.2);
 }
 
 void Odometry::setPos(float x, float y, float theta)
@@ -33,6 +35,13 @@
     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;
@@ -51,6 +60,7 @@
 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();
 }
@@ -58,7 +68,8 @@
 void Odometry::update()
 {
     DigitalOut myled(LED1);
-    myled = 1;
+    myled = 1;        
+    
     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;
@@ -67,10 +78,28 @@
     float deltaS = (m_radiusPerTick_left*delta_left + m_radiusPerTick_right*delta_right) / 2.0;
     float deltaTheta = (m_radiusPerTick_right*delta_right - m_radiusPerTick_left*delta_left) / m_v;
     
-    x += deltaS*cos(theta);
-    y += deltaS*sin(theta);
+    float dx = deltaS*cos(theta);;
+    float dy = deltaS*sin(theta);
+    
+    x += dx;
+    y += dy;
     theta += deltaTheta;
-    myled = 0;
+    
+    //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();
+    
+    myled = 0;    
 }