Diff: main.cpp

Revision:

11:8ec858b7c6d1

Parent:

10:9fb3feb38746

Child:

12:3cd91e150d9c

--- a/main.cpp	Mon Dec 10 20:12:41 2018 +0000
+++ b/main.cpp	Wed Dec 12 17:20:16 2018 +0000
@@ -11,7 +11,7 @@
 #include "FATFileSystem.h"
 #include "SimpleShell.h"
 #include "Config.h"
-#include "L3G4200D.h"
+#include "Updater.h"
 
 Serial pc(USBTX, USBRX);
 //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
@@ -21,16 +21,8 @@
 SimpleShell sh;
 
 DigitalOut led1(LED1);
-DigitalOut led2(LED2);
 Thread thread;
 
-typedef struct {
-    int g[3]; // gyro
-    float dt;  // delta time
-} sensor_data_t;
-
-sensor_data_t data;
-
 /******** SHELL COMMANDS ********/
 
 void test() {
@@ -39,51 +31,18 @@
 
 void read_gyro() 
 {
-    printf("Gyro: %d, %d, %d %f\n", data.g[0], data.g[1], data.g[2], data.dt);
+    Updater *u = Updater::instance();
+    
+    printf("Gyro: %d\n", u->get_gyro());
+    
+    //printf("Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
 }
 
-/********** PERIODIC FUNCTIONS ***********/
-L3G4200D gyro(p9, p10);
-Timer timer;
-int thisTime = 0;
-int lastTime = -1;
-
-void periodic()
-{
-    // Compute dt
-    thisTime = timer.read_us();
-    data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
-    lastTime = thisTime;
-
-    // Read encoders
-    // Read gyro
-    gyro.read(data.g);
-    //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);
-
-    led2 = !led2;
-
-    return;
-}
-
-
 /******** MAIN ********/
 
 // main() runs in its own thread in the OS
 int main()
-{
-    EventQueue *queue = mbed_highprio_event_queue();
-    
-    timer.start();
-    
+{   
     printf("Bootup...\n");
     fflush(stdout);
     
@@ -119,13 +78,9 @@
     sh.attach(read_gyro, "gyro");
     thread.start(callback(&sh, &SimpleShell::run));
 
-    //ticker.attach(callback(periodic), 0.010); // call periodic every 10ms
-
-    // schedule calls to periodic at 20 Hz
-    Event<void()> event(queue, periodic);
-    event.period(50);
-    event.post();
-    queue->dispatch_forever();
+    // Startup updater
+    Updater *u = Updater::instance();
+    u->start(50);
     
 /*
     FILE *fp;