Diff: quadCommand/quadCommand.cpp

Revision:

59:9dfd9169a5e7

Parent:

58:983801808a94

Child:

60:6906a96344a0

--- a/quadCommand/quadCommand.cpp	Wed Jul 03 03:51:45 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Wed Jul 03 04:45:03 2013 +0000
@@ -6,7 +6,13 @@
 
 quadCommand::quadCommand()
 { 
-        motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
+        // Initialize variables to zero
+        targetThrottle  = currentThrottle   = 0;    
+        targetPitch     = currentPitch      = 0;
+        targetRoll      = currentRoll       = 0;
+        targetYaw       = currentYaw        = 0;
+        pitchTrim       = 0;
+        rollTrim        = 0;
      
         myCom = new com( TXPIN, RXPIN );            // Setup com object.       
         world = new sensors();                      // Setup the sensors.
@@ -19,15 +25,9 @@
         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;
-        targetRoll      = currentRoll       = 0;
-        targetYaw       = currentYaw        = 0;
-        pitchTrim       = 0;
-        rollTrim        = 0;
+        myMotors[3] = new motor( MOTOR4 );  // Connect motor 3 to PTA5 pin.  
+        
+        motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);    
 }
 
 /******************************* run() ***********************************/
@@ -83,6 +83,12 @@
     if( command[0] == 8 )      // Pitch Trim Back
         pitchTrim -= TRIM_VALUE;
         
+    if( command[0] == 9 )      // Pitch Trim Forward
+        yawTrim += TRIM_VALUE;          
+        
+    if( command[0] == 10 )      // Pitch Trim Back
+        yawTrim -= TRIM_VALUE;
+        
     delete[] command;
 }
 
@@ -120,4 +126,5 @@
 {
     currentPitch    = world->getAbsoluteX() - pitchTrim;
     currentRoll     = world->getAbsoluteY() - rollTrim;
+    currentYaw      = 0 - yawTrim;
 }
\ No newline at end of file