testController.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file testController.cpp
*  \brief Template file for implementation of an attitude controller using the hardware simulators.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.1 $
*  \date    $Date: 2007/07/24 09:22:24 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "testController.h"

/*! /brief Constructor for testController */
testController::testController( )
{
}

/*! /brief Constructor for testController
 *
 *  @param ptr_whorl is a pointer to the Whorl object.
 */
testController::testController( Whorl* ptr_whorl )
{
        m_whorl = ptr_whorl;
        
        Initialize( );

}

/*! /brief De-Constructor for testController */
testController::~testController( ) 
{
}

/*! /brief Initialize testController */
int testController::Initialize( ) 
{
        /* Initialize control vector */
        m_controlTorque.initialize(3); // [N-m]

        /* Specify Max Wheel Torque */
        m_uMax = 1.5; // [N-m]
        
        /* Initialize contorl gain matrix */
        m_gainMatrix.initialize(3,3);
        m_gainMatrix(1,1) = 0;
        m_gainMatrix(2,2) = 0;
        m_gainMatrix(3,3) = 2;
        
       /* Get orientation of Momentum Wheels. Defined in Controller Class. */
        A = FindA( );

        /* Get Momentum Wheel spin axis MOI. Defined in Controller Class */
        MOI_sw = FindMOI_sw( );

        /* Reference Model. Initialized in Controller.cpp */
        Quaternion qri( 0.0, 0.0, 0.0, 1.0 );
        m_quaternionReference(1) = qri(1);
        m_quaternionReference(2) = qri(2);
        m_quaternionReference(3) = qri(3);
        m_quaternionReference(4) = qri(4);
        m_mrpReference = ModifiedRodriguezParameters( qri );
        m_angularRateReference(1) = 0.0; // [rad/s]
        m_angularRateReference(2) = 0.0; // [rad/s]
        m_angularRateReference(3) = 0.0; // [rad/s] 

        return( 0 );
}

int testController::Run()
{

        cerr << "Start of testController algorithm" << endl;

        /* Attitude Reference Model */

        /* Error Vectors */
        Vector angularRateError(3);
        ModifiedRodriguezParameters mrpError;

       /* Set Whorl Error Values */
        m_whorl->SetMRPError( mrpError );
        m_whorl->SetAngularRateError( angularRateError );

        /* Set Whorl Reference Parameters */
        m_whorl->SetReferenceOmegaBL( m_angularRateReference );
        m_whorl->SetReferenceQuaternion( m_quaternionReference );

       /* Reaction Wheel Speed */
        static Vector wheelSpeed(3);
        double measurementTime;
        m_whorl->GetMomentumWheel("REACTION_X")->GetWheelSpeed( wheelSpeed(1), measurementTime );
        m_whorl->GetMomentumWheel("REACTION_Y")->GetWheelSpeed( wheelSpeed(2), measurementTime );
        m_whorl->GetMomentumWheel("REACTION_Z")->GetWheelSpeed( wheelSpeed(3), measurementTime );

        /* Control law */
        m_controlTorque(1) = 0; // [N-m]
        m_controlTorque(2) = 0; // [N-m]
        m_controlTorque(3) = 0; // [N-m]

        /* Wheel Saturation Function. Max control specified in constructor. */
        m_controlTorque = WheelSaturation( m_controlTorque ); // [N-m]

        /* Set the desird contorl torque in the whorl object */
        m_whorl->SetControl( m_controlTorque ); // [N-m]

        /* Set the torque for the momentum wheels to produce */
        SetWheelTorque( m_controlTorque ); // [N-m]

        cerr << "End of testController algorithm" << endl;

        return( 0 );
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: testController.cpp,v $
*       Revision 1.1  2007/07/24 09:22:24  jayhawk_hokie
*       Initial submission.
*       
*       
*       
*
******************************************************************************/

---