Diff: quadCommand/quadCommand.cpp

Revision:

34:d6696dc977a2

Parent:

26:826448361267

Parent:

33:4f62a7a46e71

Child:

40:8c01bf294768

Child:

41:40e432c5cbe6

--- a/quadCommand/quadCommand.cpp	Mon Jun 10 00:57:48 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 01:13:40 2013 +0000
@@ -3,6 +3,7 @@
 /*************************************************************************/
 
 #include "quadCommand.h"
+#include "debug.h"
 
 const float quadCommand::MOTOR_UPDATE(20.0f);
 
@@ -68,9 +69,18 @@
     pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
     roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
     yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
+    
+    DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }", throttle, pitch, roll, yaw);)
 
-     myMotors[ 0 ]->setSpeed( throttle + pitch - roll - yaw);    // Motor 1
-     myMotors[ 1 ]->setSpeed( throttle + pitch + roll + yaw);    // Motor 2
-     myMotors[ 2 ]->setSpeed( throttle - pitch + roll - yaw);    // Motor 3
-     myMotors[ 3 ]->setSpeed( throttle - pitch - roll + yaw);    // Motor 4
+    int speed0 = throttle + pitch - roll - yaw;
+    int speed1 = throttle + pitch + roll + yaw;
+    int speed2 = throttle - pitch + roll - yaw;
+    int speed3 = throttle - pitch - roll + yaw;
+    
+    DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]", speed0, speed1, speed2, speed3);)
+    
+    myMotors[ 0 ]->setSpeed(speed0);    // Motor 1
+    myMotors[ 1 ]->setSpeed(speed1);    // Motor 2
+    myMotors[ 2 ]->setSpeed(speed2);    // Motor 3
+    myMotors[ 3 ]->setSpeed(speed3);    // Motor 4
 }
\ No newline at end of file