AngularRateController.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file AngularRateController.cpp
*  \brief Implementation of the AngularRateController class.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.1 $
*  \date    $Date: 2007/08/31 15:52:56 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "AngularRateController.h"

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

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

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

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

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

        m_uMax =1.5; // [N-m]

        double 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( ( 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]

        ssfTime time;
        time = Now( );
        m_previousTime = time.GetSeconds( );

        return( 0 );
}

int AngularRateController::Run( )
{
        ssfTime currentTime;
        currentTime = Now( );
        ssfTime deltaTime;
        deltaTime = ( currentTime.GetSeconds( ) - m_previousTime );
        //m_previousTime = currentTime.GetSeconds( ); // update time
        // dont' update time, thus provides duration time in seconds.

        /* Desired State. Reference Model */
        //ReferenceModelSC( deltaTime );
        ReferenceTrajectory( deltaTime );

        ModifiedRodriguezParameters mrp( m_whorl->GetMRP( ) );
        ModifiedRodriguezParameters mrpReference( m_mrpReference );

        /* Determine Rotation Matirx */
        Rotation RBN( mrp ); // body relative to inertial frame
        Rotation RRN( mrpReference ); // reference relative to inertial frame
        Rotation RBR; // body relative to reference frame
        RBR = RBN*~RRN;

        /* Error */
        Vector angularRateError(3);
        angularRateError = m_whorl->GetOmegaBL( ) - RBR*(m_angularRateReference); // angular rate error [rad/s]
        ModifiedRodriguezParameters mrpError;
        mrpError = mrp - mrpReference; // mrp error
        mrpError.Switch( 1 ); // important to activate switch condition

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

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

        /* Angular Rate Controller - angular rate for any simulator */
/*      m_controlTorque = (~A)*( -(m_gainMatrix) * m_whorl->GetOmegaBL( ) 
                + skew( m_whorl->GetOmegaBL( ) )*(A)*hs ); // [N-m]
*/       
        /* tracking Angular Rate Control law - attitude and angular rate */
        m_controlTorque = (~A)*( - (m_gainMatrix)*angularRateError
                + skew( m_whorl->GetOmegaBL( ) )*(m_whorl->GetMOI( )*m_whorl->GetOmegaBL( ) + (A)*hs)
                + m_whorl->GetMOI( )*( RBR.GetDCM( )*(m_angularAccelReference)
                - skew( m_whorl->GetOmegaBL( ) )*RBR.GetDCM( )*(m_angularRateReference) ) ); // [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: AngularRateController.cpp,v $
*       Revision 1.1  2007/08/31 15:52:56  jayhawk_hokie
*       Initial Submission.
*       
*       
*
******************************************************************************/

---