Diff: quadCommand/quadCommand.cpp

Revision:

58:983801808a94

Parent:

56:a550127695b2

Child:

59:9dfd9169a5e7

diff -r c29f2dac3903 -r 983801808a94 quadCommand/quadCommand.cpp
--- a/quadCommand/quadCommand.cpp	Wed Jul 03 02:40:28 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Wed Jul 03 03:51:45 2013 +0000
@@ -4,21 +4,23 @@
 
 #include "quadCommand.h"
 
-quadCommand::quadCommand() :
-    pidPitch(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
-    pidRoll(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
-    pidYaw(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD)
-{
+quadCommand::quadCommand()
+{ 
         motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
      
         myCom = new com( TXPIN, RXPIN );            // Setup com object.       
         world = new sensors();                      // Setup the sensors.
         
-        myMotors[0] = new motor( MOTOR1 );          // Connect motor 0 to PTD4 pin.
-        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. 
-                
+        // Create PID instances.
+        pidPitch = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
+        pidRoll = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
+        pidYaw = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
+        
+        myMotors[0] = new motor( MOTOR1 );  // Connect motor 0 to PTD4 pin.
+        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. 
+                       
         // Initialize variables to zero
         targetThrottle  = currentThrottle   = 0;    
         targetPitch     = currentPitch      = 0;
@@ -92,21 +94,22 @@
 void quadCommand::updateMotors() 
 {
     updateCurrent();
-    float throttle, pitch, roll, yaw;
-    
+    float throttle, pitch, roll, yaw;   
+        
     throttle = targetThrottle;
-    pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
-    roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
-    yaw = pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
+    pitch = pidPitch->correct(currentPitch, targetPitch, MOTOR_UPDATE);
+    roll = pidRoll->correct(currentRoll, targetRoll, MOTOR_UPDATE);
+    yaw = pidYaw->correct(currentYaw, targetYaw, MOTOR_UPDATE);
 
     int go = 0;
     if( targetThrottle > 0 )
         go = 1;
    
-    myMotors[ 0 ]->setSpeed( go * ( throttle - roll - pitch + yaw));    // Motor 1
-    myMotors[ 1 ]->setSpeed( go * ( throttle + roll - pitch - yaw));    // Motor 2
-    myMotors[ 2 ]->setSpeed( go * ( throttle + roll + pitch + yaw));    // Motor 3
-    myMotors[ 3 ]->setSpeed( go * ( throttle - roll + pitch - yaw));    // Motor 4
+    // Set motor speeds
+    myMotors[ 0 ]->setSpeed( go * ( throttle - roll - pitch + yaw));   
+    myMotors[ 1 ]->setSpeed( go * ( throttle + roll - pitch - yaw));    
+    myMotors[ 2 ]->setSpeed( go * ( throttle + roll + pitch + yaw));    
+    myMotors[ 3 ]->setSpeed( go * ( throttle - roll + pitch - yaw));
 }
 
 /************************** updateCurrent() ******************************/