Diff: quadCommand/quadCommand.cpp

Revision:

49:f202fb0d4128

Parent:

47:adc1a438aa33

Child:

50:197a18e49eb4

--- a/quadCommand/quadCommand.cpp	Mon Jun 10 02:18:56 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Mon Jun 10 04:43:30 2013 +0000
@@ -5,16 +5,16 @@
 #include "quadCommand.h"
 #include "debug.h"
 
-const float quadCommand::MOTOR_UPDATE(20.0f);
-
 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)
 {
-        //sensorProcess.attach( this,&quadCommand::sendTelemetry, 2 );
-        sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSORDELAY );
+        motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
+        sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSORDELAY/1000 );
+        sendProcess.attach( this, &quadCommand::sendTelemetry, SENDDELAY/1000 );
+
         
         myCom = new com( TXPIN, RXPIN );        // Setup com object.       
         world = new sensors();                  // Setup the sensors.
@@ -70,7 +70,8 @@
 /*Called in main by motorprocess                                         */
 /*************************************************************************/
 
-void quadCommand::updateMotors() {
+void quadCommand::updateMotors() 
+{
     float throttle, pitch, roll, yaw;
     
     throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
@@ -78,11 +79,11 @@
     roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
     yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
     
-    DEBUG(pc.printf("----------------------------------------");)
+    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("----------------------------------------");)
+    DEBUG(pc.printf("\r\n----------------------------------------\r\n");)
 
     float speed0 = throttle + pitch - roll - yaw;
     float speed1 = throttle + pitch + roll + yaw;
@@ -103,7 +104,8 @@
 
 void quadCommand::sendTelemetry()
 {
-    
+    myCom->write( 2, (short)(currentPitch * 900 / 10 ) );
+    myCom->write( 3, (short)(currentRoll * 900 / 10 ) );
 }
 
 /************************** updateCurrent() ******************************/