File content as of revision 34:d6696dc977a2:

/************************ quadCommand.cpp ********************************/
/*                                                                       */
/*************************************************************************/

#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)
{
        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.
}

/******************************* run() ***********************************/
/* Called from main                                                      */
/*************************************************************************/

void quadCommand::run() 
{   
    while(1)
    {      // Checks if there's data from serial
        if( myCom->isData() ) 
            rxInput();
    }
}

/***************************** rxInput() *********************************/
/*Call from run()                                                        */
/*************************************************************************/

void quadCommand::rxInput()
{
    short * command = myCom->read();
            
    if( command[0] == 1 )       // Throttle command.
        targetThrottle = command[1];
                
    if( command[0] == 2 )       // Pitch command.
        targetPitch = command[1];
                
    if( command[0] == 3 )       // Roll command.
        targetRoll = command[1];            
            
     if( command[0] == 4 )      // Yaw command.
        targetYaw = command[1];
        
     delete[] command;
}

/*************************** updateMotors() ******************************/
/*Called in main by motorprocess                                         */
/*************************************************************************/

void quadCommand::updateMotors() {
    float throttle, pitch, roll, yaw;
    
    throttle = 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);
    
    DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }", throttle, pitch, roll, yaw);)

    int speed0 = throttle + pitch - roll - yaw;
    int speed1 = throttle + pitch + roll + yaw;
    int speed2 = throttle - pitch + roll - yaw;
    int speed3 = throttle - pitch - roll + yaw;
    
    DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]", 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
}