Controller.cpp

Go to the documentation of this file.

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

#include "Controller.h"

Controller::Controller( ) 
{ 
        m_quaternionReference.initialize(4);
        m_angularRateReference.initialize(3); 
        m_angularAccelReference.initialize(3); 
        m_mrpReference.initialize(3);
} 

Controller::Controller(Whorl* ptr_whorl)
{
        m_whorl = ptr_whorl;
        A = FindA();
        MOI_sw = FindMOI_sw();

        m_quaternionReference.initialize(4);
        m_angularRateReference.initialize(3); 
        m_angularAccelReference.initialize(3); 
        m_mrpReference.initialize(3);

}

Controller::~Controller( )
{
}

Matrix Controller::FindA()
{
        
        static int wheel_num = m_whorl->GetMomentumWheel("REACTION_X")->GetWheelCount();
        Matrix temp(wheel_num,3);
        string wheel("REACTION_");
        string axis;
        string name;
        for (int ii =1;ii<=wheel_num;ii++)
        {
                // TEMP - FIND A BETTER WAY TO INCREMENT AXIS
                if (ii==1) axis = "X";
                if (ii==2) axis = "Y";
                if (ii==3) axis = "Z";          
                
                //sprintf(number,"%d",ii);
                string name =wheel+axis;
                MomentumWheel *whl = m_whorl->GetMomentumWheel(name);
                Vector tempVect = whl->GetWheelAxis();
                temp(_,ii)=tempVect;
        }                       
        return temp;
}

Matrix Controller::FindMOI_sw()
{
        
        static int wheel_num = m_whorl->GetMomentumWheel("REACTION_X")->GetWheelCount();
        Matrix temp(wheel_num,wheel_num);
        string wheel("REACTION_");
        string axis;
        string name;
        for (int ii =1;ii<=wheel_num;ii++)
        {
                if (ii==1) axis="X";
                if (ii==2) axis="Y";
                if (ii==3) axis="Z";

                //sprintf(number,"%d",ii);
                string name =wheel+axis;
                temp(ii,ii)=m_whorl->GetMomentumWheel(name)->GetAxialInertia();
        }                       
        return temp;
}

int Controller::SetWheelTorque(Vector Torque)
{
        static int wheel_num = m_whorl->GetMomentumWheel("REACTION_X")->GetWheelCount();
        string wheel("REACTION_");
        string axis;
        string name;
        for (int ii =1;ii<=wheel_num;ii++)
        {
                if (ii==1) axis="X";
                if (ii==2) axis="Y";
                if (ii==3) axis="Z";
                
                //sprintf(number,"%d",ii);
                string name = wheel+axis;
                m_whorl->GetMomentumWheel(name)->SetWheelTorque(Torque(ii));
        }
        return 0;
}


int Controller::SetThrusterTorque(char Thruster_set, int Number_of_Pulses, double Pulse_legth)
{
        return 0;
}

void Controller::ReferenceModelSC( ssfTime deltaTime )
{
        // Setup an integrator and any special parameters
        RungeKuttaIntegrator myIntegrator;
        myIntegrator.SetNumSteps(10);

        // Integration times
        vector<ssfTime> integrationTimes;
        ssfTime begin(0);
        ssfTime end( begin + deltaTime.GetSeconds( ) );
        integrationTimes.push_back(begin);
        integrationTimes.push_back(end);

        // Create the initial attitude state
        Vector state(7);
        state( _(1,4) ) = m_quaternionReference;
        state( _(5,7) ) = m_angularRateReference; 
        
        SpecificFunctor AttitudeForcesFunctor( &NullFunctor );

        // Create the matrix of parameters needed for the RHS - MOI
        Matrix Parameters(3,3);
        Parameters = m_whorl->GetMOI( );

        // Integrate over the desired time interval
        Matrix history = myIntegrator.Integrate(
                             integrationTimes,           // seconds
                             &AttituteDynamics_QuaternionAngVel,
                             state,
                             NULL,
                             NULL,
                             Parameters,
                             AttitudeForcesFunctor
                         );

        m_quaternionReference = ~history( 11, _(2,5) );
        m_angularRateReference = ~history( 11, _(6,8) ); 

        Quaternion quaternion( m_quaternionReference );
        ModifiedRodriguezParameters mrpReference( quaternion );
        mrpReference.Switch( 1 ); // force switching if greater than 180 deg.
        m_mrpReference = mrpReference;

        state( _(1,4) ) = m_quaternionReference;
        state( _(5,7) ) = m_angularRateReference; 
        Vector derivative = AttituteDynamics_QuaternionAngVel( end, state, NULL, NULL, Parameters, AttitudeForcesFunctor );
        m_angularAccelReference = derivative( _(5,7) );
        
        return;
}

// Torque function that will be called each timestep
Vector NullFunctor(const ssfTime& _pSSFTime, const OrbitState& _pOrbitState, const AttitudeState& _pAttitudeState)
{
        return Vector(3);
}


void Controller::ReferenceTrajectory( ssfTime deltaTime )
{
        // For Wheel Bias Ramp up speed.
        if ( deltaTime.GetSeconds( ) < 25 )
        {
                deltaTime = 0;
        }
        else
        { 
                deltaTime = deltaTime-25;
        }

        double time = deltaTime.GetSeconds( ); // [s]

        // Setup an integrator and any special parameters
        RungeKuttaIntegrator myIntegrator;
        myIntegrator.SetNumSteps(10);

        // Integration times
        vector<ssfTime> integrationTimes;
        ssfTime begin(deltaTime);
        ssfTime end( begin + 0.5 );
        integrationTimes.push_back(begin);
        integrationTimes.push_back(end);

        // Create the initial attitude state
        Vector state(4);
        state( _(1,4) ) = m_quaternionReference;
        
        SpecificFunctor AttitudeForcesFunctor( &NullFunctor );

        // Create the matrix of parameters needed for the RHS - MOI
        Matrix Parameters(3,3);
        Parameters = m_whorl->GetMOI( );
        // Integrate over the desired time interval
        Matrix history = myIntegrator.Integrate(
                             integrationTimes,           // seconds
                             &AttitudeTrajectory,
                             state,
                             NULL,
                             NULL,
                             Parameters,
                             AttitudeForcesFunctor
                         );
        
        m_quaternionReference = ~history( 11, _(2,5) );
        //m_angularRateReference = ~history( 11, _(6,8) ); 

        double omega = 0.025; // [rad/s]
        double C1 = 30;
        double C2 = -0.1; 
        m_angularRateReference(1) = 0; //sin( omega*time ); 
        m_angularRateReference(2) = 0; //sin( omega*time ); 
        m_angularRateReference(3) = exp( -time/C1 )*sin( omega*time ) + C2; 

        Quaternion quaternion( m_quaternionReference );
        ModifiedRodriguezParameters mrpReference( quaternion );
        mrpReference.Switch( 1 ); // force switching if greater than 180 deg.
        m_mrpReference = mrpReference;

        //Vector derivative = AttitudeTrajectory( end, state, NULL, NULL, Parameters, AttitudeForcesFunctor );
        //m_angularAccelReference = derivative( _(5,7) );
        m_angularAccelReference(1) = 0;  
        m_angularAccelReference(2) = 0;  
        m_angularAccelReference(3) = -exp( -time/C1 )*sin( omega*time )/C1 + omega*exp( -time/C1 )*cos( omega*time ); 
        
        return;
}


Vector AttitudeTrajectory(const ssfTime &_time, const Vector& _state, Orbit *_Orbit, Attitude *_Attitude, const Matrix &_parameters, const Functor &_torqueFuncPtr)
{
        static Vector stateDot(4);

        static Vector _quaternionRate(4);

        static Quaternion q;
        q = _state( _(1,4) );

        static Quaternion qRate;

        double time = _time.GetSeconds( );
        
        double omega = 0.025; // [rad/s]
        double C1 = 30;
        double C2 = -0.1; 
        static Vector angularRate(3);
        angularRate(1) = 0; //sin( omega*time ); 
        angularRate(2) = 0; //sin( omega*time ); 
        angularRate(3) = exp( -time/C1 )*sin( omega*time ) + C2; 
        /*
        angularAccel(1) = 0;  
        angularAccel(2) = 0;  
        angularAccel(3) = -exp( -time/C1 )*sin( omega*time )/C1 + omega*exp( -time/C1 )*cos( omega*time ); 
        */

        AttitudeModels Rate;
        Rate.QuaternionKinematic( q, angularRate, qRate );

        stateDot( _(1,4) ) = qRate;


        return (stateDot);
}


Vector Controller::Saturation( Vector _s, Vector _phi )
{
        Vector _sat(3);

        for( int iterator = 1; iterator < 4; iterator++ )
        {
                if( fabs( _s( iterator ) ) <= _phi( iterator ) )
                {
                        _sat( iterator ) = _s( iterator )/_phi( iterator );
                }
                else
                {
                        _sat( iterator ) = SignSingle( _s( iterator ) );
                }
        }
        return( _sat );
}

Vector Controller::Sign( Vector _s )
{
        Vector _sign(3);
        
        for( int iterator = 1; iterator < 4; iterator++ )
        {
                _sign( iterator ) = SignSingle( _s( iterator ) );
        }
        return( _sign );
}


Vector Controller::WheelSaturation( Vector _u )
{
        Vector _uSat(3);

        for( int iterator = 1; iterator < 4; iterator++ )
        {
                if( fabs( _u( iterator ) ) <= m_uMax )
                {
                        _uSat( iterator ) = _u( iterator );
                }
                else
                {
                        _uSat( iterator ) = m_uMax*SignSingle( _u( iterator ) );
                }
        }
        return( _uSat );
}



double Controller::SignSingle( double _s )
{
        if( _s >= 0 )
        {
                _s = 1;
        }
        else
        {
                _s = -1;
        }

        return( _s );
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: Controller.cpp,v $
*       Revision 1.10  2007/08/31 15:53:26  jayhawk_hokie
*       Modified.
*       
*       Revision 1.9  2007/07/24 09:26:11  jayhawk_hokie
*       Added attitude reference models.
*       
*       Revision 1.8  2007/05/23 22:18:10  jayhawk_hokie
*       Implemented spacecraft reference model.
*       
*       Revision 1.7  2007/03/27 19:42:05  jayhawk_hokie
*       Removed unused variables.
*       
*       Revision 1.6  2007/02/09 14:34:33  jayhawk_hokie
*       Added XML wheel parsing technique.
*       
*       Revision 1.5  2003/08/13 23:16:42  mavandyk
*       Altered structure as outline in meeting.
*       
*       Revision 1.4  2003/08/01 11:19:00  mavandyk
*       Changed the class structure, and fixed bugs.
*       
*       Revision 1.3  2003/07/07 14:11:24  simpliciter
*       Removed GetWhorlObject() calls following Whorl pointers.
*       
*       
*
******************************************************************************/

---