Diff: main.cpp

Revision:

6:b3baf0fe5b73

Parent:

5:d2e955a94940

Child:

9:7a8fb72f9a93

diff -r d2e955a94940 -r b3baf0fe5b73 main.cpp
--- a/main.cpp	Wed Mar 25 09:58:50 2015 +0000
+++ b/main.cpp	Wed Mar 25 11:40:32 2015 +0000
@@ -1,69 +1,93 @@
+#include "mbed.h"
+#include "rtos.h"
 #include "robot.h"
-#include "bt_shell.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];
 
+    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;
+    */
+
+}
 
 void motor_t(void const *args)
-{   
-    double angle = 1;
-    while(1){
-        //move the servo so it reflects the IMU ROLL
-        if (((t.read()- prev)*1000) > write_rate){
-            angle = imu_data.ypr[1]/90;
-            myservo = angle;  
-            //angle = angle - 0.01;
-            //if (angle < 0)
-            //    angle = 1;
-            prev = t.read();
-        }
-        Thread::wait(10);
-    }
+{
+//    myservo = 0;
 }
 
 void IMU2_thread(void const *args)
 {
     test_dmp2();
     start_dmp2(mpu2);
-    while(1)
-    {
+    while(1) {
         update_dmp2();
-         Thread::yield();
+        Thread::yield();
     }
 }
 void IMU_thread(void const *args)
 {
-    bt.baud(BT_BAUD_RATE);
     test_dmp();
-    bt.baud(BT_BAUD_RATE);
     start_dmp(mpu);
-    
+
     while(1) {
         update_dmp();
-         Thread::yield();
+        Thread::yield();
     }
 }
 
 void bt_shell(void const *args)
 {
-    bt_shell_init();
-    bt.printf("BT shell initialized\r\n");
-    
+    btSwitch = 1;
+    pc.baud(115200);
+
+    int command = 0;
     while(true) {
-        bt_shell_run();
-        Thread::wait(10);
-    }    
+//        if(pc.readable()) {
+//            pc.scanf("%d;", &command);
+//        }
+//
+//        pc.printf("%.3f;%d\r\n", getTime(), command);
+        print_all();
+        Thread::wait(50);
+    }
 }
 
 int main()
 {
-    initRobot();
+    t.start();
 
     Thread bt_shell_th(bt_shell, 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); 
-    
+    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);
+
     Thread::wait(osWaitForever);
 }
\ No newline at end of file