MomentumWheel.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file MomentumWheel.cpp
*  \brief Implementation of the MomentumWheel class
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.35 $
*  \date    $Date: 2007/07/24 09:39:39 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "MomentumWheel.h"

int MomentumWheel::m_WheelCount = 0;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

MomentumWheel::MomentumWheel()
{
        ++m_WheelCount;
}


MomentumWheel::MomentumWheel( TiXmlHandle handle, string wheel ) 
{
        Initialize( handle, wheel );
}

MomentumWheel::~MomentumWheel()
{
        Deinitialize( );
}

//////////////////////////////////////////////////////////////////////
//  Facilitators
/////////////////////////////////////////////////////////////////////

int MomentumWheel::Initialize( TiXmlHandle handle, string wheel ) 
{
        ++m_WheelCount;

        m_WheelAxis.initialize(3);

        m_exitConditionData = 1;
        
        int daisy_chain;
        int wheel_address;
        int response;
        double torque_limit_low; 
        double torque_limit_high;
        double axialInertia;
        double BiasSpeed;
        
        Vector wheel_axis(3);
        
        string port;

        TiXmlHandle wheelHandle = handle.FirstChild("HARDWARE_PROPERTIES").FirstChild("WHEELS").FirstChild(wheel);

        response = wheelHandle.Child("DAISY_CHAIN",0).Element() -> QueryIntAttribute("value", &daisy_chain);
        checkResponse(response);

        response = wheelHandle.Child("DCADDR", 0).Element() -> QueryIntAttribute("value", &wheel_address);
        checkResponse(response);

        response = wheelHandle.Child("LIMIT_LOW", 0).Element() -> QueryDoubleAttribute("value", &torque_limit_low);
        checkResponse(response);

        response = wheelHandle.Child("LIMIT_HIGH", 0).Element() -> QueryDoubleAttribute("value", &torque_limit_high);
        checkResponse(response);

        response = wheelHandle.Child("INERTIA", 0).Element() -> QueryDoubleAttribute("value", &axialInertia);
        checkResponse(response);

        response = wheelHandle.Child("BIAS_SPEED", 0).Element() -> QueryDoubleAttribute("value", &BiasSpeed);
        checkResponse(response);

        for (int i=1; i<4; i++)
        {
                response = wheelHandle.Child("AXIS", i-1).Element() -> QueryDoubleAttribute( "value", &wheel_axis(i) );
        }

        port = wheelHandle.Child("PORT", 0).Element() -> Attribute("value");

        // Get Wheel Speed Coefficients
        Vector Coefficients1(4);
        for (int i=1; i<5; i++)
        {
                response = wheelHandle.Child("COEFFICIENTS1", i-1).Element() -> QueryDoubleAttribute( "value", &Coefficients1(i) );
        }
        Vector Coefficients2(3);
        for (int i=1; i<4; i++)
        {
                response = wheelHandle.Child("COEFFICIENTS2", i-1).Element() -> QueryDoubleAttribute( "value", &Coefficients2(i) );
        }
        Vector Coefficients3(3);
        for (int i=1; i<4; i++)
        {
                response = wheelHandle.Child("COEFFICIENTS3", i-1).Element() -> QueryDoubleAttribute( "value", &Coefficients3(i) );
        }
        Vector Coefficients4(4);
        for (int i=1; i<5; i++)
        {
                response = wheelHandle.Child("COEFFICIENTS4", i-1).Element() -> QueryDoubleAttribute( "value", &Coefficients4(i) );
        }
        // Set Wheel Speed Coefficients
        SetWheelSpeedCoefficients( Coefficients1, Coefficients2, Coefficients3, Coefficients4 );
        
        // set local and physical momentum wheel values

        SetChainNumber( daisy_chain );
        SetWheelAddress( wheel_address );
        SetPort( port );
        SetWheelTorqueLimits( torque_limit_low, torque_limit_high );
        SetWheelAxis( wheel_axis );
        SetAxialInertia( axialInertia );
        SetBiasSpeed( BiasSpeed );      
        SetWheelName( wheel );

        m_PhysicalMomentumWheel.Initialize( );
  
        s_mapStringValues["ON"]                 = ON;
        s_mapStringValues["OFF"]                = OFF;
        // Determine whether to log wheel speed.
        m_LogData = wheelHandle.Child( "LOGGING", 0 ).Element() -> Attribute( "value" );

        pthread_mutex_init( &m_DataMutex, NULL );
        
        switch(  s_mapStringValues[ m_LogData ] )
        {
                case ON:
                        m_WheelHistory = new WheelHistory( handle, wheel );
                        StartWheelSpeedDataLogging( );
                        break;
                case OFF:
                        break;
                default:
                        break;
        }

        return( 0 );
}

int MomentumWheel::Deinitialize( )
{
        --m_WheelCount;
    
        // Delete Pointer       
        switch(  s_mapStringValues[ m_LogData ] )
        {
                case ON:
                        StopWheelSpeedDataLogging( );
                        delete m_WheelHistory;
                        break;
                case OFF:
                        break;
                default:
                        break;
        }
        
        //delete m_WheelAxis;

        //m_PhysicalMomentumWheel.Deinitialize( );
        return( 0 );
}

int MomentumWheel::Stop()
{
        m_PhysicalMomentumWheel.Stop();
        return( 0 );
}

int MomentumWheel::TorqueCommand( double _power )
{
        m_PhysicalMomentumWheel.TorqueCommand( _power );
        return( 0 );
}


void MomentumWheel::StartSpeedQuery( )
{
        m_PhysicalMomentumWheel.StartSpeedQuery( );
}

void MomentumWheel::StopSpeedQuery( )
{
        m_PhysicalMomentumWheel.StopSpeedQuery( );
}

void MomentumWheel::SetWheelTorque( double wheelTorque )
{
        m_PhysicalMomentumWheel.CommandWheelTorque( wheelTorque );
}

void MomentumWheel::SetWheelSpeed(double wheelOmega)
{
        m_PhysicalMomentumWheel.SetWheelSpeed( wheelOmega );
        m_PhysicalMomentumWheel.CommandWheelSpeed( );
}

void MomentumWheel::StartPID( )
{
        m_PhysicalMomentumWheel.StartPID( );
}

void MomentumWheel::StopPID( )
{
        m_PhysicalMomentumWheel.StopPID( );
}

void MomentumWheel::SetSystemTorque( double wheelTorque )
{
        m_PhysicalMomentumWheel.CommandSystemTorque( wheelTorque );
}


////////////////////////////////////////////////////////////////
//Mutators
/////////////////////////////////////////////////////////////////

void MomentumWheel::SetWheelAddress( int wheeladdress )
{
        m_PhysicalMomentumWheel.SetWheelAddress( wheeladdress );
}

void MomentumWheel::SetWheelAxis( const Vector& newWheelAxis ) 
{
    m_WheelAxis = newWheelAxis;
}

void MomentumWheel::SetChainNumber( int daisyChain )
{
        m_PhysicalMomentumWheel.SetDaisyChainNumber( daisyChain );
}

void MomentumWheel::SetPort( string Port )
{
        m_PhysicalMomentumWheel.SetDaisyChainPort( Port );
}

void MomentumWheel::SetBiasSpeed( double BiasSpeed )
{
        m_PhysicalMomentumWheel.SetBiasSpeed( BiasSpeed );
}

void MomentumWheel::SetAxialInertia( double axialInertia )
{
        m_PhysicalMomentumWheel.SetAxialInertia( axialInertia );
}

void MomentumWheel::SetWheelTorqueLimits( double torque_limit_low, double torque_limit_high )
{
        m_PhysicalMomentumWheel.SetWheelTorqueLimits( torque_limit_low, torque_limit_high );
}

void MomentumWheel::SetWheelName( string _wheelName )
{
        m_PhysicalMomentumWheel.SetWheelName( _wheelName );
}

void MomentumWheel::SetWheelSpeedCoefficients( Vector _Coefficients1, Vector _Coefficients2, Vector _Coefficients3, Vector _Coefficients4 )
{
        m_PhysicalMomentumWheel.SetWheelSpeedCoefficients( _Coefficients1, _Coefficients2, _Coefficients3, _Coefficients4 );
}


/////////////////////////////////////////////////////////////////////
//Inspectors
////////////////////////////////////////////////////////////////////////


void MomentumWheel::GetWheelSpeed( double& Speed, double& measTime ) 
{
        m_PhysicalMomentumWheel.GetWheelSpeed( Speed, measTime );
}


double MomentumWheel::GetWheelTorque( )
{
        return( m_PhysicalMomentumWheel.QueryWheelTorque( ) );
}

Vector MomentumWheel::GetWheelAxis( ) const 
{
        return( m_WheelAxis );
}


int MomentumWheel::GetWheelCount( )
{
        return( m_WheelCount );
}


double MomentumWheel::GetAxialInertia( )
{
        return( m_PhysicalMomentumWheel.GetAxialInertia( ) );
}

string MomentumWheel::GetWheelName( )
{
        return( m_PhysicalMomentumWheel.GetWheelName( ) );
}

int MomentumWheel::GetWheelFD()
{
        return( m_PhysicalMomentumWheel.GetWheelFD( ) );
}

double MomentumWheel::GetAmps()
{
        return( m_PhysicalMomentumWheel.GetAmps( ) );
}

double MomentumWheel::GetVolts()
{
        return( m_PhysicalMomentumWheel.GetVolts( ) );
}


/////////////////////////////////////////////////////////////////////
// Data Logging
////////////////////////////////////////////////////////////////////////

void MomentumWheel::StartWheelSpeedDataLogging( )
{
        pthread_create( &m_DataSpeedThreadID, NULL, DataWheelSpeed, (void*) this );
        return;
}

void* DataWheelSpeed( void* arg )
{
        MomentumWheel *wheel = (MomentumWheel*) arg;

        // in future should be a variable
        int readDelay = 50000; // [usec]

        double measurementTime = 0;
        ssfTime measurementTime2 = 0;
        double wheelSpeed = 0;

        int exit = 1;

        while( exit )
        {
                pthread_testcancel( );
                wheel -> GetWheelSpeed( wheelSpeed, measurementTime );
                measurementTime2= measurementTime;
                
                // Lock Reaction Wheel Parameters
                pthread_mutex_lock(&wheel->m_DataMutex);
                        wheel -> m_WheelHistory -> AppendHistory( measurementTime2, wheelSpeed );
                // Unlock Reaction Wheel Parameters
                pthread_mutex_unlock(&wheel->m_DataMutex);

                pthread_testcancel( );
    
                pthread_mutex_unlock( &wheel->m_DataMutex );
                        exit = wheel->m_exitConditionData;
                pthread_mutex_unlock( &wheel->m_DataMutex );
        
                pthread_testcancel( );
        
                usleep( readDelay );
        } 
      return arg;
}

void MomentumWheel::StopWheelSpeedDataLogging( )
{
        pthread_mutex_unlock( &m_DataMutex );
                m_exitConditionData = 0;
        pthread_mutex_unlock( &m_DataMutex );
        
        pthread_join( m_DataSpeedThreadID, NULL );
        return;
}




// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: MomentumWheel.cpp,v $
*       Revision 1.35  2007/07/24 09:39:39  jayhawk_hokie
*       Removed matrix and vector pointers.
*       
*       Revision 1.34  2007/05/30 23:48:43  jayhawk_hokie
*       Modified thread termination.
*       
*       Revision 1.33  2007/03/27 19:07:16  jayhawk_hokie
*       Updated Momentum Wheels.
*       
*       Revision 1.32  2007/02/06 20:48:16  jayhawk_hokie
*       Update for XML parsing for Momentum Wheels.
*       
*       Revision 1.31  2005/02/25 18:40:52  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.30  2004/05/11 20:18:08  bstreetman
*       Vast improvements to torque controller
*       
*       Revision 1.29  2004/04/19 15:00:38  bstreetman
*       Fixed a config and a shutdown bug
*       
*       Revision 1.28  2004/03/04 23:13:08  bstreetman
*       Big changes! new functions and functionality.  FUnctions: GetVolts() -- get motor voltage.  GetAmps() -- ge current used by motors.  GetWheelFD() -- get the wheel comm file descriptor.  SetBiasSpeed() -- Set the bias speed used in momentum wheel mode.  SetSystemTorque() -- old style torque function, used mainly by me.  friend RUnTorqueCOntroller() -- new torque controller, runs in a separate thread.  StartTorqueController() -- spawns torque thread, called internally.  Other Changes:  config parameters -- added BiasSpeed confige parameter, sets bias speed and commands the wheel to go there.  SetWheelSpeed() -- no more PID controller, uses a function created by the data i took.  SetWHeelTorque() -- happens in a better way now based on tons of data and hdot = g.
*       
*       Revision 1.27  2003/12/16 17:20:26  bstreetman
*       A bit o' fall cleaning.  Just look at how pretty it is.
*       
*       Revision 1.26  2003/11/14 20:15:44  bstreetman
*       Apparently I broke the PID controller with the last update, but now it works again.  I also added a StopPID() function to end the thread of the PID controller.  To spin up to 900 rpm (about half speed) use SetWheelSpedd(92).  If you wnat to then switch to torque control sleep for about 12 seconds and run StopPID() and you should be good to go.
*       
*       Revision 1.25  2003/10/17 20:01:34  bstreetman
*       Wheel code updated to reflect hardware changes.  All wheels now considered on separate serial port.  Wheels also changed to read wheel velocity in a separate thread.  Thread is fired off by StartSpeedQuery() function.  Wheel Speed and measurement timestamp are set by the function GetWheelSpeed(speed, time).  See SpeedTest.cpp in the work directory for a sample using the new code.
*       
*       Revision 1.24  2003/09/15 21:44:03  bstreetman
*       Removed hardcoded parameters, ready for pure config file use
*       
*       Revision 1.23  2003/08/18 19:30:43  mavandyk
*       Fixed config parsing.
*       
*       Revision 1.22  2003/08/06 21:18:48  bstreetman
*       Updated to work with config parser
*       
*       Revision 1.21  2003/08/04 18:11:35  bstreetman
*       Updated port locations
*       
*       Revision 1.20  2003/08/01 20:21:20  bstreetman
*       Cleaned and streamlined all code. Reduced warning and outputs. CMG and Momentum Wheels now no longer accept a global integer in initialization.  The file descriptor is now set and held internally.
*       
*       Revision 1.19  2003/07/28 17:54:00  bstreetman
*       Change some pass-by-reference to pass-by-value
*       
*       Revision 1.18  2003/07/21 20:15:30  bstreetman
*       Updates
*       
*       Revision 1.17  2003/07/18 19:51:18  cakinli
*       Fixed file footer.
*       
*       Revision 1.2  2003/06/27 19:52:14  bstreetman
*       Defined all remaining functions and updated their implementations.
*       
*       Revision 1.1.1.1  2003/06/06 18:44:15  simpliciter
*       Initial submission.
*       
*
******************************************************************************/

---