Diff: main.cpp

Revision:

9:7a8fb72f9a93

Parent:

6:b3baf0fe5b73

Child:

10:22c44650c7c1

--- a/main.cpp	Wed Mar 25 11:44:05 2015 +0000
+++ b/main.cpp	Wed Mar 25 14:34:23 2015 +0000
@@ -2,41 +2,12 @@
 #include "rtos.h"
 #include "robot.h"
 #include "DMP.h"
-
-Serial pc(PTA2,PTA1);
-DigitalOut btSwitch(PTE30);
-
-void print_all()    //call this function to print all sensor data
-{
-    float data[13];
-    data[0]= getTime();
-    data[1]= imu_data.ypr[0];
-    data[2]= imu_data.ypr[1];
-    data[3]= imu_data.ypr[2];
-
-    data[4]= imu2_data.ypr[0];
-    data[5]= imu2_data.ypr[1];
-    data[6]= imu2_data.ypr[2];
+#include "Sensor.hpp"
+#include "control.hpp"
+#include "io.h"
 
-    pc.printf("&;");
-    pc.printf("%f;",data[0]);       //print time as an integer (milliseconds)
-    for(int i = 1; i<=6; i++) {
-        pc.printf("%.3f;",data[i]);
-    }
-    pc.printf("\n\r");
-    //commented out the below code because we want to ignore the rear IR sensors
-    /*dt = t.read() - data[0];
-    irBack = 1;
-    if(dt < 50)
-        Thread::wait(50-dt);
-
-    data[6]= irR.read();
-    data[7]= irL.read();
-    data[8]= imu_data.ypr[0]*180/PI;
-    irBack = 0;
-    */
-
-}
+Sensor sensor;
+DigitalOut btSwitch(PTE30);
 
 void motor_t(void const *args)
 {
@@ -63,20 +34,25 @@
     }
 }
 
-void bt_shell(void const *args)
+void io_thread(void const *args)
 {
     btSwitch = 1;
     pc.baud(115200);
+    setup(sensor);
 
-    int command = 0;
     while(true) {
-//        if(pc.readable()) {
-//            pc.scanf("%d;", &command);
-//        }
-//
-//        pc.printf("%.3f;%d\r\n", getTime(), command);
-        print_all();
-        Thread::wait(50);
+        if(pc.readable() && pc.getc() == 'c') {
+            sensor.calibrate();
+            setStateToStop();
+        }
+        
+        process();
+        
+        pc.printf(
+            "%.3f;%.3f;%.3f;%i;%.3f;%.3f;%.3f;\r\n", 
+            sensor.time(), sensor.thigh(), sensor.shin(), getState(), getPace(), 0, 0
+        );
+        Thread::wait(10);
     }
 }
 
@@ -84,7 +60,7 @@
 {
     t.start();
 
-    Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
+    Thread bt_shell_th(io_thread, NULL, osPriorityNormal, 2048, NULL);
     Thread IMU_th(IMU_thread, NULL, osPriorityNormal, 2048, NULL);
     Thread IMU2_th(IMU2_thread, NULL, osPriorityNormal, 2048, NULL);
     Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL);