Diff: quadCommand/quadCommand.cpp

Revision:

47:adc1a438aa33

Parent:

45:088885f4a13d

Parent:

46:f1eb22f41ebc

Child:

49:f202fb0d4128

--- a/quadCommand/quadCommand.cpp	Mon Jun 10 02:12:02 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 02:17:16 2013 +0000
@@ -78,12 +78,16 @@
     roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
     yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
     
+    DEBUG(pc.printf("----------------------------------------");)
+    DEBUG(pc.printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);)
+    DEBUG(pc.printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);)
     DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);)
+    DEBUG(pc.printf("----------------------------------------");)
 
-    int speed0 = throttle + pitch - roll - yaw;
-    int speed1 = throttle + pitch + roll + yaw;
-    int speed2 = throttle - pitch + roll - yaw;
-    int speed3 = throttle - pitch - roll + yaw;
+    float speed0 = throttle + pitch - roll - yaw;
+    float speed1 = throttle + pitch + roll + yaw;
+    float speed2 = throttle - pitch + roll - yaw;
+    float speed3 = throttle - pitch - roll + yaw;
     
     DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]\r\n", speed0, speed1, speed2, speed3);)