Diff: main.cpp

Revision:

2:430c068cf570

Parent:

0:ff9bc5f69c57

Child:

3:666c1bae1a34

diff -r ff9bc5f69c57 -r 430c068cf570 main.cpp
--- a/main.cpp	Tue Jan 21 21:32:05 2014 +0000
+++ b/main.cpp	Fri Jan 24 21:55:00 2014 +0000
@@ -1,23 +1,41 @@
 #include "mbed.h"
 #include "motor_controller.h"
+#include "guardian.h"
 #include "IMU.h"
+#include "Servo.h"
+#include "rtos.h"
+
+bool quit=false;
 
 PololuMController mcon(p22, p6, p5);
-IMU imu(0.1, 0.3, 0.005, 0.005);
+Servo servo(p21);
+Guardian ap(p21, p23, p24, p25, p26, p26);
+Serial xbee(p13, p14);
+Serial pc(USBTX, USBRX);
 
-int main() {
+//IMU imu(0.1, 0.3, 0.005, 0.005);
+
+void motor_thread(void const *args) {
     Timer t;
     t.start();
-    while(1) {
-       /* if (speed>=1)
-        {
-            speed=0;
-            mcon.reverse();
-        }
-        mcon.setspeed(speed);
-        */
-        mcon.drive_sinusoidal(t.read(), 1, 0.25*3.14, 0);
-        wait(0.2);
+    while (quit==false) {
+        mcon.drive_sinusoidal(t.read(), 1, 2*3.14, 0);
     }
     t.stop();
 }
+
+int main()
+{
+    Thread thread(motor_thread);
+    ap.calibrate();
+    ap.set2D();
+    while(1) {
+        char buf[128];
+        if (xbee.readable())
+        {
+            xbee.gets(buf, 128);
+            pc.puts(buf);
+        }
+        memset(buf, 0, 128);
+    }
+}