Diff: main.cpp

Revision:

2:ab967d7b4346

Parent:

0:8c28fac22d27

Child:

3:605fbcb54e75

--- a/main.cpp	Tue Apr 29 11:02:18 2014 +0000
+++ b/main.cpp	Tue Apr 29 11:43:32 2014 +0000
@@ -5,9 +5,18 @@
 
 int main(void)
 {
+    imu.debugSerial.baud(115200);
+    imu.debugSerial.printf("hello\n");
     bool error = false;
-    
+
     /* Setup devices */
+    if (setup_ESC())
+        error = false;
+    else {
+        error = true;
+        imu.debugSerial.printf("ESC FAILED!!!\n");
+    }
+
     if (setup_led())
         error = false;
     else
@@ -15,29 +24,28 @@
 
     if (setup_bt()) {
         error = false;
-        BT.printf("test\n");
-        }
-    else
+        BT.printf("BT established!\n");
+    } else
         error = true;
-        
+
     if (setup_mpu6050()) {
         error = false;
-        BT.printf("Setup done!\n");
-        }
-    else error = true;
+        BT.printf("MPU6050 established!\n");
+    } else error = true;
 
     if (!error) {
+        imu.debugSerial.printf("Devices ready!\n");
         /* Create threads */
         RtosTimer thread1(Task1, osTimerPeriodic, NULL);
         RtosTimer thread2(Task2, osTimerPeriodic, NULL);
         RtosTimer thread3(Task3, osTimerPeriodic, (void *)0);
-        //RtosTimer thread4(Task4, osTimerPeriodic, (void *)0);
+        RtosTimer thread4(Task4, osTimerPeriodic, (void *)0);
 
         /* Start threads */
         thread1.start(TASK1_PERIOD);
         thread2.start(TASK2_PERIOD);
         thread3.start(TASK3_PERIOD);
-        //thread4.start(TASK4_PERIOD);
+        thread4.start(TASK4_PERIOD);
 
         /* Execute state machine forever */
         Thread::wait(osWaitForever);