Diff: main.cpp

Revision:

0:d98bd8b7dc46

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 09 15:57:36 2018 +0000
@@ -0,0 +1,63 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "attitude.h"
+#include "LinkedList.h"
+  
+LinkedList<int>command;
+
+Serial pc(USBTX,USBRX);
+
+Timer timer1; 
+Timer timerwalk;
+
+Thread thread1;
+Thread thread2;
+
+void IMU();
+void move();
+
+int walking_time;
+int i = 1;
+int n = 0;
+
+list<int> command;
+
+int main() {
+    pc.baud(1000000);
+    timer1.start();
+    pc.printf("start\n");
+    thread1.start(move);
+    thread2.start(IMU);
+}
+
+void move(){
+    wait(5);
+    while(n<10){
+        wait(0.01);
+        pc.printf("%d\n", command(n));
+        n = n+1;
+    }
+}
+
+void IMU()
+{
+    while(1) {
+        if (timer1.read_us() >= 1000)// read time in ms
+        {
+            attitude_get();
+            //pc.printf("IMU");
+            //pc.printf(" %f \n", ax*10 ); 
+            //pc.printf("  %f \t", ay*10 ); 
+            //pc.printf("  %f \t", az*10 -10); //cm/s*s
+
+            //pc.printf(" %f \t", gx ); 
+            //pc.printf("  %f \t", gy );  
+            //pc.printf("  %f  \t", gz );  //deg/s    */
+            //pc.printf("%.0f\t  %.0f \t  %.0f \n\r",  roll,   pitch, yaw);
+            command(i) = roll;
+            i = i+1;
+            timer1.reset(); // reset timer 
+        }
+    }
+}
+