Diff: main.cpp

Revision:

37:e1ad11fe3fe4

Parent:

36:660be809a49d

--- a/main.cpp	Wed Nov 20 10:47:38 2013 +0000
+++ b/main.cpp	Wed Feb 26 08:46:22 2014 +0000
@@ -17,25 +17,11 @@
  * Output
  */
 
-/*#include "mbed.h"
-#include "mbos.h"
-
-DigitalOut myled(LED1);
-
-int main() {
-    while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
-        
-        //modification
-    }
-}*/
 #include "mbed.h"                    
 #include "mbos.h"
 #include "Module_Communication.h"
 #include "Module_Mouvement.h"
+#include "Acc_Giro.h"
  
 #define TASK1_ID                1       // Id for task 1 (idle task is 0)
 #define TASK1_PRIO              50      // priority for task 1
@@ -54,23 +40,57 @@
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 mbos os(2, 1);                          // Instantiate mbos with 2 tasks & 1 timer 
-ModuleMouvement test;   
+Acc_Giro Acc_Giro_test; 
+Serial pc(USBTX, USBRX);  
 
 int main(void)
 {
-/*
-   // Configure tasks and timers
-   os.CreateTask(TASK1_ID, TASK1_PRIO, TASK1_STACK_SZ, task1);
-   os.CreateTask(TASK2_ID, TASK2_PRIO, TASK2_STACK_SZ, task2);
-   os.CreateTimer(TIMER0_ID, TIMER0_EVENT, TASK1_ID);
-    // Start mbos
-   os.Start();
-    // never  return!
-    */
-    test.TestMotor();
-    C_ModuleCommunication com = C_ModuleCommunication();
-    com.receptionDeTrame();
+    //Initialize inertial sensors.
+    Acc_Giro_test.initializeAccelerometer();
+    Acc_Giro_test.initializeGyroscope();
+          
+    Timer tmr;
+    tmr.start();
+    
+    pc.printf("test started");
+    Acc_Giro_test.calibrate=1;
     
+    while(true)
+    {
+        //Net::poll();
+        if(tmr.read() > 0.2){
+           // led4=!led4;
+            tmr.reset();
+            wait(0.5);
+        pc.printf("Ax:%f Ay:%f Az:%f || Gx:%f Gy:%f Gz:%f\n", Acc_Giro_test.a_x, Acc_Giro_test.a_y, Acc_Giro_test.a_z, toDegrees(Acc_Giro_test.imuFilter->getRoll()), toDegrees(Acc_Giro_test.imuFilter->getPitch()), toDegrees(Acc_Giro_test.imuFilter->getYaw()));
+           
+        }
+        
+        if(Acc_Giro_test.calibrate && !Acc_Giro_test.calibrated){
+            Acc_Giro_test.calibrateAccelerometer();
+            Acc_Giro_test.calibrateGyroscope();
+            led2 = 1;
+            Acc_Giro_test.calibrated = 1;
+            Acc_Giro_test.start = 1;
+          pc.printf("Calibrated\n");
+        }
+        
+        
+        if(Acc_Giro_test.calibrated && Acc_Giro_test.start && !Acc_Giro_test.started){
+         
+            //Accelerometer data rate is 500Hz, so we'll sample at this speed.
+            Acc_Giro_test.accelerometerTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleAccelerometer, 0.002);
+            //Gyroscope data rate is 500Hz, so we'll sample at this speed.
+            Acc_Giro_test.gyroscopeTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleGyroscope,  0.002);
+            //Update the filter variables at the correct rate.
+            Acc_Giro_test.filterTicker.attach(&Acc_Giro_test, &Acc_Giro::filter, FILTER_RATE);
+            //Acc_Giro_test.dataTicker.attach(&Acc_Giro_test, &Acc_Giro::dataSender, 0.2);
+           // Acc_Giro_test.algorithmTicker.attach(&algorithm, 0.001);
+            Acc_Giro_test.started = 1;
+        }  
+        
+        
+    }  
 }
 
 void task1(void)