Diff: main.cpp

Revision:

24:fa52fd530d6e

Parent:

23:b79faf58d4a1

Child:

25:af166a728c89

--- a/main.cpp	Mon Jun 01 06:06:28 2015 +0000
+++ b/main.cpp	Tue Jun 02 06:51:34 2015 +0000
@@ -53,9 +53,11 @@
 void swaveTask(void)
 {
     if(squarewave) {
-        motor2.setAngle(0);
+        motor1.setAngle(5);
+        motor2.setAngle(5);
     } else {
-        motor2.setAngle(60);
+        motor1.setAngle(55);
+        motor2.setAngle(55);
     }
     squarewave=!squarewave;
 }
@@ -116,7 +118,6 @@
     //initialize_connection();
     infoTicker.attach(infoTask,0.2f);
    // commandTicker.attach(getCommand,0.2f);
-    swaveTicker.attach(swaveTask,1.0f);
     if(imu.isReady()) {
         debug.printf("MPU9150 is ready\r\n");
     } else {
@@ -133,12 +134,17 @@
     imu.setDMPEnabled(true);
 
     Quaternion q1;
-    wait_ms(500);
+    motor1.setAngle(-60);
+
+    motor2.setAngle(-60);
+    wait_ms(1000);
     motor1.setAngle(0);
 
     motor2.setAngle(0);
+    wait_ms(100);
     initCaptures();
 
+    swaveTicker.attach(swaveTask,.20f);
 
     while(1) {
         if(imu.getFifoCount() >= 48) {