Diff: quadCommand/quadCommand.cpp

Revision:

55:bca9c9e92da6

Parent:

53:cce34958f952

Child:

56:a550127695b2

--- a/quadCommand/quadCommand.cpp	Tue Jun 11 02:36:12 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Thu Jun 13 01:11:03 2013 +0000
@@ -3,13 +3,13 @@
 /*************************************************************************/
 
 #include "quadCommand.h"
-#include "debug.h"
 
 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)
+    // 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 );
@@ -17,20 +17,27 @@
      
         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.
-        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.
+        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;
         targetPitch     = currentPitch      = 0;
         targetRoll      = currentRoll       = 0;
         targetYaw       = currentYaw        = 0;
+        pitchTrim       = 0;
+        rollTrim        = 0;
 }
 
 /******************************* run() ***********************************/
-/* Called from main                                                      */
+/*                                                                       */
 /*************************************************************************/
 
 void quadCommand::run() 
@@ -43,7 +50,25 @@
 }
 
 /***************************** rxInput() *********************************/
-/*Call from run()                                                        */
+/* Commands:    0 -> Ack, does not get placed in rxQueue.                */
+/*              1 -> Throttle                                            */
+/*              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                                           */
 /*************************************************************************/
 
 void quadCommand::rxInput()
@@ -62,9 +87,22 @@
      if( command[0] == 4 )      // Yaw command.
         targetYaw = command[1];
         
+     if( command[0] == 5 )      // Roll Trim Left.
+        rollTrim += TRIM_VALUE;
+        
+     if( command[0] == 6 )      // Roll Trim Right.
+        rollTrim -= TRIM_VALUE;
+        
+     if( command[0] == 7 )      // Pitch Trim Forward
+        pitchTrim += TRIM_VALUE;          
+        
+     if( command[0] == 8 )      // Pitch Trim Back
+        pitchTrim -= TRIM_VALUE;
+        
      delete[] command;
 }
 
+
 /*************************** updateMotors() ******************************/
 /*Called in main by motorprocess                                         */
 /*************************************************************************/
@@ -74,21 +112,25 @@
     updateCurrent();
     float throttle, pitch, roll, yaw;
     
-    throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
+    throttle = targetThrottle; //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);
+    yaw = 0.0f; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
     
-    //DEBUG(pc.printf("\r\n----------------------------------------\r\n");)
-    //DEBUG(pc.printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);)
-    //DEBUG(pc.printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);)
-    //DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);)
-    //DEBUG(pc.printf("\r\n----------------------------------------\r\n");)
+    //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");
 
-    float speed0 = throttle + pitch - roll - yaw;
-    float speed1 = throttle + pitch + roll + yaw;
-    float speed2 = throttle - pitch + roll - yaw;
-    float speed3 = throttle - pitch - roll + yaw;
+    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);)
     
@@ -114,6 +156,6 @@
 
 void quadCommand::updateCurrent()
 {
-    currentPitch    = world->getAbsoluteX();
-    currentRoll     = world->getAbsoluteY();
+    currentPitch    = world->getAbsoluteX() - pitchTrim;
+    currentRoll     = world->getAbsoluteY() - rollTrim;
 }
\ No newline at end of file