Diff: quadCommand/quadCommand.cpp

Revision:

32:3d180aa938ba

Parent:

18:c7df2edd73f0

Child:

33:4f62a7a46e71

--- a/quadCommand/quadCommand.cpp	Mon Jun 10 00:42:18 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 01:05:51 2013 +0000
@@ -3,6 +3,7 @@
 /*************************************************************************/
 
 #include "quadCommand.h"
+#include "debug.h"
 
 const float quadCommand::MOTOR_UPDATE(20.0f);
 
@@ -68,6 +69,8 @@
     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("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