Diff: quadCommand/quadCommand.cpp

Revision:

16:84c7db5b4464

Parent:

9:9e0d0ba5b6b1

Parent:

15:92ecb025fbc5

Child:

17:d73944c3c945

--- a/quadCommand/quadCommand.cpp	Mon Jun 10 00:01:48 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 00:04:56 2013 +0000
@@ -4,7 +4,7 @@
 
 #include "quadCommand.h"
 
-const float quadCommand::MOTOR_UPDATE(20.0);
+const float quadCommand::MOTOR_UPDATE(20.0f);
 
 quadCommand::quadCommand()
 {
@@ -61,10 +61,17 @@
 /*************************************************************************/
 
 void quadCommand::updateMotors() {
-     myMotors[ 0 ]->setSpeed( rxThrottle + rxPitch - rxRoll - rxYaw);    // Motor 1
-     myMotors[ 1 ]->setSpeed( rxThrottle + rxPitch + rxRoll + rxYaw);    // Motor 2
-     myMotors[ 2 ]->setSpeed( rxThrottle - rxPitch + rxRoll - rxYaw);    // Motor 3
-     myMotors[ 3 ]->setSpeed( rxThrottle - rxPitch - rxRoll + rxYaw);    // Motor 4
+    float throttle, pitch, roll, yaw;
+    
+    throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
+    pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
+    roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
+    yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
+
+     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
                          
      delete[] command;
 }
\ No newline at end of file