Diff: Updater.cpp

Revision:

14:1dd83e626153

Parent:

13:5566df1250f1

Child:

15:35c40765f7c3

--- a/Updater.cpp	Wed Dec 12 17:38:02 2018 +0000
+++ b/Updater.cpp	Wed Dec 12 18:21:57 2018 +0000
@@ -1,17 +1,24 @@
 #include "Updater.h"
-
+#include "pinouts.h"
+#include "IncrementalEncoder.h"
 
 void Updater::gyro(int g[3], float& dt) 
 {
     for (int i=0; i < 3; i++) {
         g[i] = _gyro[i];
     }
-    
     dt = _dt;
     
     return;
 }
 
+int Updater::encoder()
+{
+    int result=_ecount;
+    _ecount = 0;
+    
+    return result;    
+}
 
 Updater *Updater::instance() 
 {
@@ -41,7 +48,8 @@
 void Updater::update()
 {
     static DigitalOut led2(LED2);
-    static L3G4200D gyro(p9, p10); // TODO parameterize
+    static L3G4200D gyro(I2CSDA, I2CSCL); // TODO parameterize
+    static IncrementalEncoder enc(ALEFT);
 
     // Compute dt
     thisTime = t->read_us();
@@ -49,6 +57,8 @@
     lastTime = thisTime;
 
     // Read encoders
+    _ecount += enc.read();
+    
     // Read gyro
     gyro.read(_gyro);