Diff: main.cpp

Revision:

11:ccb8653e285f

Parent:

7:9068fc754a0b

Child:

13:d6374484b953

--- a/main.cpp	Thu Dec 03 17:53:04 2015 +0000
+++ b/main.cpp	Sat Dec 19 13:28:17 2015 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-#include "rtos.h"
 #include "M3Dpi.h"
 #include "jsonReporter.h"
 
@@ -7,7 +6,12 @@
 
 M3Dpi robot;
 Serial pc(USBTX,USBRX);
-JsonReporter report(&pc, M3DPI_ID);
+Reporter* report = new JsonReporter(&pc, M3DPI_ID);
+
+m3dpi::Distance distance;
+m3dpi::Direction direction;
+m3dpi::Rotation rotation;
+m3dpi::Acceleration acceleration;
 
 
 void setLeds(m3dpi::Distance distance)
@@ -20,31 +24,29 @@
     robot.setLeds(colors);
 }
 
-void read_sensors_thread(void const * args)
+void read_sensors_thread()
 {
     robot.setStatus(Color::GREEN);
-    m3dpi::Distance distance = robot.getDistance();
-    setLeds(distance);
-    
-    report.distance(distance);
-    report.direction(robot.getDirection());
-    report.rotation(robot.getRotation());
-    report.acceleration(robot.getAcceleration());
-    
-    report.time(robot.getTime());
-    robot.setStatus(Color::OFF);
+    ::distance = robot.getDistance();
+    direction = robot.getDirection();
+    rotation = robot.getRotation();
+    acceleration = robot.getAcceleration();
+    setLeds(::distance);
+    robot.setStatus(Color::BLACK);
 }
 
 int main()
 {            
     pc.baud(115200);
-       
-    // for testing...
-    pc.printf("Device ID is: 0x%02x\n", robot.accelerometer.getDevId());
-    pc.printf("Gyro id: 0x%02x\n", robot.gyro.getWhoAmI());
-    
-    RtosTimer periodicThread(read_sensors_thread, osTimerPeriodic);
-    periodicThread.start(50);
-    
-    Thread::wait(osWaitForever);
+           
+    while(true){
+        read_sensors_thread();
+        report->distance(::distance);
+        report->direction(direction);
+        report->rotation(rotation);
+        report->acceleration(acceleration);
+        //pc.printf("report send time: %d us\n", t.read_us());
+        //report.time(robot.getTime());
+        wait_ms(500);
+    }
 }
\ No newline at end of file