Diff: main.cpp

Revision:

5:090ef6275773

Parent:

3:666c1bae1a34

Child:

6:a4d6f3e4bf28

--- a/main.cpp	Fri Jan 24 22:04:41 2014 +0000
+++ b/main.cpp	Tue Jan 28 23:59:21 2014 +0000
@@ -4,7 +4,7 @@
 #include "IMU.h"
 #include "Servo.h"
 #include "rtos.h"
-
+#include "PwmReader.h"
 bool quit=false;
 
 PololuMController mcon(p22, p6, p5);
@@ -13,13 +13,22 @@
 Serial xbee(p13, p14);
 Serial pc(USBTX, USBRX);
 
+
 //IMU imu(0.1, 0.3, 0.005, 0.005);
 void motor_thread(void const *args) {
-
+    
     Timer t;
     t.start();
-    while (quit==false) {
-        mcon.drive_sinusoidal(t.read(), 1, 2*3.14, 0);
+    PwmReader thrDutyCycl(p7);
+    
+    PwmReader freqDutyCycl(p8);   
+    
+    while (quit==false) {     
+        float td = thrDutyCycl.getDuty();
+        float fd = freqDutyCycl.getDuty();
+        pc.printf("throttle: %f frequency: %f\n",td,fd);
+        mcon.drive_sinusoidal(t.read(), 1, 2*3.141 ,0); // (time,amplitude,frequency,freqoffset)
+        Thread::wait(10);
     }
     t.stop();
 }
@@ -27,15 +36,20 @@
 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);
+    //ap.calibrate();
+    //ap.set2D();
+    ap.setoff();
+        while(1) {
+        //char buf[128];
+//        if (xbee.readable())
+//        {
+//            xbee.gets(buf, 128);
+//            //xbee.scanf("%d");
+//            pc.puts(buf);
+//        }
+//        memset(buf, 0, 128);
+               
+        
+        Thread::wait(10);
     }
 }