File content as of revision 0:8681037b9a18:

/************************ 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;
}