Diff: control.cpp

Revision:

5:d85bac38cdff

Parent:

4:44a5b1e8fd27

--- a/control.cpp	Sat Nov 02 08:47:14 2013 +0000
+++ b/control.cpp	Tue Nov 26 20:29:54 2013 +0000
@@ -2,18 +2,47 @@
 #include "sensor.h"
 #include "steps.h"
 #include "mbed.h"
+#include "IMUfilter.h"
 
 #define MAXPROC 15
+IMUfilter imuFilter(0.1, 10);
+
 
 process procs[MAXPROC] = {0};
-
+Serial pc3(USBTX, USBRX);
+LocalFileSystem local("local");
 int main()
 {
+
     init();
+    pc3.baud(115200);
+    accelerometer_measure();
+    gyro_turnon();
+    sensor ter;
+    int a;
     while (true)
     {
         schedule();
+        a =  read_accelerometer(&ter);
+        a = read_gyro(&ter);
+        
+          wait(1);
+        //Gyro and accelerometer values must be converted to rad/s and m/s/s
+        //before being passed to the filter.
+     /* imuFilter.updateFilter(ter.gx,
+                               ter.gy,
+                               ter.gz,
+                               ter.ax,
+                               ter.ay,
+                               ter.az);
+        imuFilter.computeEuler();
+        printf("%f, %f, %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
+    
+    */
+    
+       
     }
+;
 }
 
 void init()