Diff: quadCommand/quadCommand.cpp

Revision:

18:c7df2edd73f0

Parent:

12:15d129d681e9

Parent:

17:d73944c3c945

Child:

26:826448361267

Child:

32:3d180aa938ba

diff -r 15d129d681e9 -r c7df2edd73f0 quadCommand/quadCommand.cpp
--- a/quadCommand/quadCommand.cpp	Mon Jun 10 00:09:59 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 00:12:48 2013 +0000
@@ -4,9 +4,13 @@
 
 #include "quadCommand.h"
 
-const float quadCommand::MOTOR_UPDATE(20.0);
+const float quadCommand::MOTOR_UPDATE(20.0f);
 
-quadCommand::quadCommand()
+quadCommand::quadCommand() :
+    pidThrottle(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
+    pidPitch(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
+    pidRoll(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
+    pidYaw(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD)
 {
         myCom = new com( TXPIN, RXPIN );        // Setup com object.       
         world = new sensors();                  // Setup the sensors.
@@ -15,11 +19,6 @@
         myMotors[1] = new motor( MOTOR2 );      // Connect motor 1 to PTA12 pin.
         myMotors[2] = new motor( MOTOR3 );      // Connect motor 2 to PTA4 pin.
         myMotors[3] = new motor( MOTOR4 );      // Connect motor 3 to PTA5 pin.
-        
-        rxThrottle    = 0;
-        rxPitch       = 0;
-        rxRoll        = 0;
-        rxYaw         = 0;
 }
 
 /******************************* run() ***********************************/
@@ -63,10 +62,15 @@
 /*************************************************************************/
 
 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
-                         
-     delete[] command;
+    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
 }
\ No newline at end of file