Diff: Updater.cpp

Revision:

11:8ec858b7c6d1

Child:

12:3cd91e150d9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Updater.cpp	Wed Dec 12 17:20:16 2018 +0000
@@ -0,0 +1,59 @@
+#include "Updater.h"
+
+
+Updater *Updater::instance() {
+    static Updater instance;
+
+    return &instance;
+}
+
+
+void Updater::start(int interval_ms)
+{
+    Timer timer;
+    
+    thisTime = 0;
+    lastTime = 0; 
+    t = &timer;
+    t->start();
+   
+    EventQueue *queue = mbed_highprio_event_queue();
+    Event<void()> event(queue, callback(this, &Updater::update));
+    event.period(interval_ms);
+    event.post();
+    queue->dispatch_forever();
+}
+
+
+void Updater::update()
+{
+    static DigitalOut led2(LED2);
+    static L3G4200D gyro(p9, p10); // TODO parameterize
+
+    // Compute dt
+    thisTime = t->read_us();
+    //data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
+    lastTime = thisTime;
+
+    // Read encoders
+    // Read gyro
+    int g_a[3];
+    gyro.read(g_a);
+    g = g_a[2];
+    
+    //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
+
+    // Save current data into history fifo to use 1 second from now
+    //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
+    //history[now].gyro = sensors.gyro[_z_];  // current raw gyro data
+    //history[now].dt = dt; // current dt, to address jitter
+    //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
+    //float r = PI/180 * history[now].hdg;
+    //history[now].x = history[prev].x + history[now].dist * sin(r);    // update current position
+    //history[now].y = history[prev].y + history[now].dist * cos(r);
+
+    // Convert this into some kind of status message/event thingy
+    led2 = !led2;
+
+    return;
+}
\ No newline at end of file