Diff: main.cpp

Revision:

10:9fb3feb38746

Parent:

9:fc3575d2cbbf

Child:

11:8ec858b7c6d1

--- a/main.cpp	Fri Dec 07 15:58:41 2018 +0000
+++ b/main.cpp	Mon Dec 10 20:12:41 2018 +0000
@@ -20,10 +20,16 @@
 Config config;
 SimpleShell sh;
 
-L3G4200D gyro(p9, p10);
 DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+Thread thread;
 
-Thread thread;
+typedef struct {
+    int g[3]; // gyro
+    float dt;  // delta time
+} sensor_data_t;
+
+sensor_data_t data;
 
 /******** SHELL COMMANDS ********/
 
@@ -33,21 +39,39 @@
 
 void read_gyro() 
 {
-    int g[3];
-    gyro.read(g);
-    printf("Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
+    printf("Gyro: %d, %d, %d %f\n", data.g[0], data.g[1], data.g[2], data.dt);
 }
 
+/********** PERIODIC FUNCTIONS ***********/
+L3G4200D gyro(p9, p10);
+Timer timer;
+int thisTime = 0;
+int lastTime = -1;
 
-/******** GYRO UPDATER ********/
-void gyro_update()
+void periodic()
 {
-    int g[3];
- 
-    while (1) {
-        gyro.read(g);
-        // send message / add to status fifo?
-    }
+    // 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;
 }
 
 
@@ -56,12 +80,14 @@
 // main() runs in its own thread in the OS
 int main()
 {
+    EventQueue *queue = mbed_highprio_event_queue();
+    
+    timer.start();
+    
     printf("Bootup...\n");
     fflush(stdout);
     
     printf("Loading config...\n");
-    
-    // Add valid keywords
     config.add("intercept_distance", Config::DOUBLE);
     config.add("waypoint_threshold", Config::DOUBLE);
     config.add("minimum_turning_radius", Config::DOUBLE);
@@ -87,12 +113,20 @@
     if (config.load("/etc/2018cfg.txt")) {
         printf("error loading config\n");
     }
-    
+
+    // Startup shell    
     sh.attach(test, "test");
     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();
+    
 /*
     FILE *fp;
     char buf[128];