DefaultController.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file DefaultController.cpp
*  \brief Implementation of the DefaultController class.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.10 $
*  \date    $Date: 2007/08/31 15:53:26 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "DefaultController.h"

/* Constructor */
DefaultController::DefaultController( )
{ 
}

DefaultController::DefaultController( Whorl* ptr_whorl ) 
{
        /* Set Maximum control torque for wheels. */
        m_uMax = 1.5; // [N-m]

        // From Controller Class
        m_whorl = ptr_whorl;
        
        Initialize( );
        
}

/* Deconstructor */
DefaultController::~DefaultController() 
{ }

int DefaultController::Initialize( ) 
{
        /* Initialize control torque */
        m_controlTorque.initialize(3); // [N-m]

        m_uMax =1.5; // [N-m]

        /* Gain */
        m_K = 1.5;

        /* Set the contorl gain matrix. semi automatic gain selection */
        m_gainMatrix.initialize(3,3);
        Matrix _inertiaTemp(3,3);
        _inertiaTemp = m_whorl->GetMOI( );
        for ( int i=1; i<4; i++ )
        {
                m_gainMatrix(i,i) = pow( ( m_K* _inertiaTemp(i,i) ), 0.5 );
        }
        
        /* Get orientation of Momentum Wheels */
        A = FindA( );

        /* Get Momentum Wheel spin axis MOI */
        MOI_sw = FindMOI_sw( );

        /* Reference Model, Regulator problem, therefore desired states are zero. */
        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 DefaultController::Run( )
{

        Vector wheelSpeed(3);
        double meastime;
        m_whorl->GetMomentumWheel("REACTION_X")->GetWheelSpeed( wheelSpeed(1), meastime );
        m_whorl->GetMomentumWheel("REACTION_Y")->GetWheelSpeed( wheelSpeed(2), meastime );
        m_whorl->GetMomentumWheel("REACTION_Z")->GetWheelSpeed( wheelSpeed(3), meastime );
        
        // For continuity store values that are used at multiple times.
        Vector angularRateBody(3);
        angularRateBody = m_whorl->GetOmegaBL( ); 
        
        Quaternion attitude( m_whorl-> GetQuaternion( ) );
        ModifiedRodriguezParameters mrp( attitude );
        mrp.Switch( 1 );

        /* Set Whorl Error Values */
        m_whorl->SetMRPError( mrp );
        m_whorl->SetAngularRateError( angularRateBody );

        /* Set Whorl Reference Parameters */
        m_whorl->SetReferenceOmegaBL( m_angularRateReference );
        m_whorl->SetReferenceQuaternion( m_quaternionReference );
        
        /* Reaction Wheel Angular Momentum */
        Vector hs(3);
        Vector w(3);
        Matrix ws(1,1);
        for ( int i=1; i<4; i++ )
        {
                w(1) = A(1,i); 
                w(2) = A(2,i); 
                w(3) = A(3,i); 
                ws = ~w*m_whorl->GetOmegaBL( );
                hs(i) = MOI_sw(i,i) * ( ws(1,1) + wheelSpeed(i) );
        }
        
        /* MRP Controller - attitude and angular rate for any simulator */
        m_controlTorque = (~A)*( -m_K*(mrp) + -(m_gainMatrix) * angularRateBody 
                + skew( angularRateBody )*(A)*hs ); // [N-m]

        /* Wheel Saturation Function */
        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]

        return( 0 );
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: DefaultController.cpp,v $
*       Revision 1.10  2007/08/31 15:53:26  jayhawk_hokie
*       Modified.
*       
*       Revision 1.9  2007/07/24 09:36:44  jayhawk_hokie
*       Updated.
*       
*       Revision 1.8  2007/05/30 23:45:34  jayhawk_hokie
*       Cleaned up file and set error values.
*       
*       Revision 1.7  2007/04/09 14:03:06  jayhawk_hokie
*       Modified controller.
*       
*       Revision 1.6  2007/02/06 21:47:05  jayhawk_hokie
*       Update Run Function.
*       
*       Revision 1.5  2005/02/25 18:40:53  cakinli
*       Created Makefiles and organized include directives to reduce the number of
*       include paths.  Reorganized libraries so that there is now one per source
*       directory.  Each directory is self-contained in terms of its Makefile.
*       The local Makefile in each directory includes src/config.mk, which has all
*       the definitions and general and pattern rules.  So at most, to see what
*       goes into building a target, a person needs to examine the Makefile in
*       that directory, and ../config.mk.
*       
*       Revision 1.4  2003/08/18 19:05:27  mavandyk
*       Removed wheel speed query to speed up controller.
*       
*       Revision 1.3  2003/08/14 18:10:41  mavandyk
*       Added momentum wheel speed querying.
*       
*       Revision 1.2  2003/08/13 23:17:20  mavandyk
*       Altered structure as outline in meeting.
*       
*       Revision 1.1  2003/07/31 20:43:14  mavandyk
*       This is the default controller for Whorl.
*       
*       Revision 1.3  2003/07/07 14:11:24  simpliciter
*       Removed GetWhorlObject() calls following Whorl pointers.
*       
*       
*
******************************************************************************/

---