Diff: quadCommand/quadCommand.cpp

Revision:

33:4f62a7a46e71

Parent:

32:3d180aa938ba

Child:

34:d6696dc977a2

--- a/quadCommand/quadCommand.cpp	Mon Jun 10 01:05:51 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 01:12:52 2013 +0000
@@ -70,10 +70,17 @@
     roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
     yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
     
-    DEBUG(pc.printf("t = %f, p = %f, r = %f, y = %f", throttle, pitch, roll, yaw);)
+    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