Diff: quadCommand/quadCommand.cpp

Revision:

0:8681037b9a18

Child:

1:9b90e7de6e09

diff -r 000000000000 -r 8681037b9a18 quadCommand/quadCommand.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadCommand/quadCommand.cpp	Sun Jun 09 22:13:59 2013 +0000
@@ -0,0 +1,98 @@
+/************************ quadCommand.cpp ********************************/
+/*                                                                       */
+/*************************************************************************/
+
+#include "quadCommand.h"
+
+quadCommand::quadCommand()
+{
+        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.
+        
+        rxThrottle    = 0;
+        rxPitch       = 0;
+        rxRoll        = 0;
+        rxYaw         = 0;
+}
+
+void quadCommand::run() 
+{   
+    while(1)
+    {
+        if( myCom->isData() )
+            rxInput();
+            
+        //myCom->write( 3, world->getAbsoluteY());       
+        updatePosition();
+         
+        if( globalUpdate )
+            txPosition();
+    }
+}
+
+void quadCommand::rxInput()
+{
+    short * command = myCom->read();
+            
+    if( command[0] == 1 )       // Throttle command.
+        rxThrottle = command[1];
+                
+    if( command[0] == 2 )       // Pitch command.
+        rxPitch = command[1];
+                
+    if( command[0] == 3 )       // Roll command.
+        rxRoll = command[1];            
+            
+     if( command[0] == 4 )      // Yaw command.
+        rxYaw = command[1];           
+            
+     myMotors[ 0 ]->setSpeed( rxThrottle + rxPitch - rxRoll );    // Motor 1
+     myMotors[ 1 ]->setSpeed( rxThrottle + rxPitch + rxRoll );    // Motor 2
+     myMotors[ 2 ]->setSpeed( rxThrottle - rxPitch + rxRoll );    // Motor 3
+     myMotors[ 3 ]->setSpeed( rxThrottle - rxPitch - rxRoll );    // Motor 4
+                         
+     delete[] command;
+}
+
+void quadCommand::updatePosition()
+{
+    float temp;
+    
+    temp = world->getAbsoluteX();
+    if( temp != currentRoll )
+    {
+        currentRoll = temp;
+        updatedRoll = true;
+        globalUpdate = true;
+    }
+    
+    temp = world->getAbsoluteY();
+    if( temp != currentPitch )
+    {
+        currentPitch = temp;
+        updatedPitch = true;
+        globalUpdate = true;
+    }
+}
+
+void quadCommand::txPosition()
+{
+    if( updatedRoll )
+    {
+        myCom->write( 3, currentRoll);
+        updatedRoll = false;
+    }
+        
+    if( updatedPitch )
+    {
+        myCom->write( 2, currentPitch);
+        updatedPitch = false;
+    }
+    
+    globalUpdate = false;
+}
\ No newline at end of file