Diff: quadCommand/quadCommand.cpp

Revision:

15:92ecb025fbc5

Parent:

8:72791d8c36b7

Child:

16:84c7db5b4464

--- a/quadCommand/quadCommand.cpp	Sun Jun 09 23:36:31 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 00:03:12 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()
 {
@@ -49,10 +49,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