Diff: quadCommand/quadCommand.cpp

Revision:

56:a550127695b2

Parent:

55:bca9c9e92da6

Child:

58:983801808a94

--- a/quadCommand/quadCommand.cpp	Thu Jun 13 01:11:03 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Tue Jul 02 21:03:46 2013 +0000
@@ -5,30 +5,22 @@
 #include "quadCommand.h"
 
 quadCommand::quadCommand() :
-    // Proportional, Intagral, Derivitive, Windup
-    pidThrottle(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
     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)
 {
         motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
-        //sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSOR_DELAY/1000 );
-        //sendProcess.attach( this, &quadCommand::sendTelemetry, SEND_DELAY/1000 );
      
         myCom = new com( TXPIN, RXPIN );            // Setup com object.       
         world = new sensors();                      // Setup the sensors.
-        //pc = new Serial(USBTX, USBRX);
         
-        myMotors[0] = new motor( MOTOR1 );          // Connect motor 0 to PTD4 pin. // motor 2
-        myMotors[1] = new motor( MOTOR2 );          // Connect motor 1 to PTA12 pin. // motor 1
-        myMotors[2] = new motor( MOTOR3 );          // Connect motor 2 to PTA4 pin. // motor 3
-        myMotors[3] = new motor( MOTOR4 );          // Connect motor 3 to PTA5 pin. // motor 4
-        
-        myMotors[1]->setPulseMin(0.0011850);
-        myMotors[2]->setPulseMin(0.00116875);
-        myMotors[3]->setPulseMin(0.0011625);
-        
-        targetThrottle  = currentThrottle   = 0;
+        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;
@@ -55,20 +47,10 @@
 /*              2 -> Pitch                                               */
 /*              3 -> Roll                                                */
 /*              4 -> Yaw                                                 */
-/*              5 -> Motor 1 trimUp                                      */
-/*              6 -> Motor 1 trimDown                                    */
-/*              7 -> Motor 1 setTrim                                     */
-/*              8 -> Motor 2 trimUp                                      */
-/*              9 -> Motor 2 trimDown                                    */
-/*              10-> Motor 2 setTrim                                     */
-/*              11-> Motor 3 trimUp                                      */
-/*              12-> Motor 3 trimDown                                    */
-/*              13-> Motor 3 setTrim                                     */
-/*              14-> Motor 4 trimUp                                      */
-/*              15-> Motor 4 trimDown                                    */
-/*              16-> Motor 4 setTrim                                     */
-/*              17-> valueUp                                             */
-/*              18-> valueDown                                           */
+/*              5 -> Roll Trim Left                                      */
+/*              6 -> Roll Trim Right                                     */
+/*              7 -> Pitch Trim Forward                                  */
+/*              8 -> Pitch Trim Back                                     */
 /*************************************************************************/
 
 void quadCommand::rxInput()
@@ -84,22 +66,22 @@
     if( command[0] == 3 )       // Roll command.
         targetRoll = command[1];            
             
-     if( command[0] == 4 )      // Yaw command.
+    if( command[0] == 4 )      // Yaw command.
         targetYaw = command[1];
-        
-     if( command[0] == 5 )      // Roll Trim Left.
+            
+    if( command[0] == 5 )      // Roll Trim Left.
         rollTrim += TRIM_VALUE;
         
-     if( command[0] == 6 )      // Roll Trim Right.
+    if( command[0] == 6 )      // Roll Trim Right.
         rollTrim -= TRIM_VALUE;
         
-     if( command[0] == 7 )      // Pitch Trim Forward
+    if( command[0] == 7 )      // Pitch Trim Forward
         pitchTrim += TRIM_VALUE;          
         
-     if( command[0] == 8 )      // Pitch Trim Back
+    if( command[0] == 8 )      // Pitch Trim Back
         pitchTrim -= TRIM_VALUE;
         
-     delete[] command;
+    delete[] command;
 }
 
 
@@ -112,42 +94,19 @@
     updateCurrent();
     float throttle, pitch, roll, yaw;
     
-    throttle = targetThrottle; //pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
+    throttle = targetThrottle;
     pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
     roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
-    yaw = 0.0f; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
-    
-    //pc->printf("\r\n----------------------------------------\r\n");
-    //pc->printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);
-    //pc->printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);
-    //pc->printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);
-    //pc->printf("\r\n----------------------------------------\r\n");
+    yaw = pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
 
     int go = 0;
     if( targetThrottle > 0 )
         go = 1;
-
-    float speed0 = go * (throttle - roll - pitch + yaw);
-    float speed1 = go * (throttle + roll - pitch - yaw);
-    float speed2 = go * (throttle + roll + pitch - yaw);
-    float speed3 = go * (throttle - roll + pitch + yaw);
-    
-    //DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]\r\n", speed0, speed1, speed2, speed3);)
-    
-    myMotors[ 0 ]->setSpeed(speed0);    // Motor 1
-    myMotors[ 1 ]->setSpeed(speed1);    // Motor 2
-    myMotors[ 2 ]->setSpeed(speed2);    // Motor 3
-    myMotors[ 3 ]->setSpeed(speed3);    // Motor 4
-}
-
-/**************************** sendData() *********************************/
-/*                                                                       */
-/*************************************************************************/
-
-void quadCommand::sendTelemetry()
-{
-    myCom->write( 2, (short)(currentPitch * 90) );
-    myCom->write( 3, (short)(currentRoll * 90) );
+   
+    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
 }
 
 /************************** updateCurrent() ******************************/