PhysicalMomentumWheel.h

Go to the documentation of this file.

/************************************************************************************************/
//SubInclude DSACSSROOT src Base ;
/*! \file PhysicalMomentumWheel.h
*  \brief The PhysicalMomentumWheel class provides the interface for h/w momentum wheels.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.35 $
*  \date    $Date: 2007/07/24 09:39:39 $
************************************************************************************************/
/*! 
*
************************************************************************************************/

#ifndef __SSSL_PHYSICALMOMENTUMWHEEL_H__
#define __SSSL_PHYSICALMOMENTUMWHEEL_H__

#include <iostream>
#include <sstream>
#include <fstream>


// Standard System Header Files
#include <pthread.h>
#include <string>
#include <sstream>
#include <fstream>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/ioctl.h>
#include <asm/ioctls.h>
#include <bits/types.h>

// DSACSS Header Files
#include <Base/Actuator.h>
#include <utils/Time.h>
extern "C"
{
        #include <Hardware/satctl.h>
}
extern "C"
{
        #include <Hardware/common.h>
}

// Open-SESSAME Header Files
#include <matrix/Matrix.h>

using namespace std;
using namespace O_SESSAME;

/*! Class to interact with actual harware momentum wheels */
class PhysicalMomentumWheel : public Actuator  
{
public:
        /////////////////////////////////////////////////////
        // Contructors/Deconstructors
        ////////////////////////////////////////////////////

        /*! \brief Default Constructor 
         */
        PhysicalMomentumWheel( );

        /*! \brief Default Deconstructor 
         */
        virtual ~PhysicalMomentumWheel( );

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

        /*! \brief Initialize the wheel and its chain
         */
        int Initialize( );

        /*! \brief Deinitialize the wheel and its chain.
         */
        int Deinitialize( );

        /*! \brief Total System Reset for wheel motor.
         */
        int Reset( );

        int ChangeBaud( );

        /*! \brief Set ECHO to on. Echo input data back out main channel
         */
        int EchoOn( );

        /*! \brief Set ECHO to off. Do not echo input data back out main channel
         */
        int EchoOff( );

        /*! \brief Enable torque mode for the wheel motor.
         */
        int EnableTorqueMode( );

        /*! \brief Enable torque mode for the wheel motor.
         *  @param _power is the voltage to be commanded in Torque Mode 
         */
        int TorqueCommand( double _power );
        
        /*! \brief Stop the wheel. The function resets the settings for the wheel motor.
         *  Not a good method, but works.
         */
        int Stop( );
        
        /*! \brief Start the speed reading routine of the motor in a separate thread
         *  Each wheel's speed query needs to be started individually.  The performance occurs
         *  when the spart calls for each wheel are spread evenly.
         */
        void StartSpeedQuery( );

        /*! \brief Friend function split in a new thread that queries the wheel speed repeatedly.  It begins running in the 
         *  StartSpeedQuery function. Velocity by Position: This function determines the velocity of the wheel by differentiating
         *  the position reported by the wheel.  The function asks the wheel for its
         *  position and then gets the time of day at which the measurement is reported.
         *  The code then delays for a small time and asks for the position and gets
         *  the time again.  The velocity is then calculated by dividing the change
         *  in position byt he change in time.  The return value is the velocity in rev/sec.
         *
         *  @param arg the argument is set to be a "this" pointer cast as a void pointer 
         */
        friend void* QueryWheelSpeed( void* arg );

        /*! \brief Stop the speed reading routine of the motor.  This function does not need to be called a t the end of the program.
         *  The thread will be closed automatically when the program ends. 
         */
        void StopSpeedQuery( );

        /*! \brief Command a torque to the wheel
         *  @param _wheelTorque double representing the desired torque output in N-m 
         */
        void CommandWheelTorque( double _wheelTorque );
        
        /*! \brief Start the torque controller.  It is called in the CommandWheelTorque function.
         */
        void StartTorqueController( );
        
        /*! Friend function split in a new thread that controls the torque applied.  It begins running in the 
         *  StartTorqueController function.  Determine reaction wheel speed to provide the desired torque requested from controller.
         *  @param arg the argument is set to be a "this" pointer cast as a void pointer 
         */
        friend void* RunTorqueController( void* arg );
    
        /*! Friend function split in a new thread that controls the torque applied.  It begins running in the 
         *  StartTorqueController function.  Determine reaction wheel speed to provide the desired torque requested from controller.
         *  @param arg the argument is set to be a "this" pointer cast as a void pointer 
         */
        friend void* RunTorqueControllerRev( void* arg );

        /*! \brief End the Torque Controller Thread.
         */
        void StopTorqueController( );

        /*! \brief Set wheel speed.
         *  @param desiredWheelSpeed
         */
        void SetWheelSpeed( double desiredWheelSpeed );

        /*! \brief Command a speed to the wheel.
         *  Retrieves wheelspeed from double m_DesiredWheelSpeed. 
         */
        void CommandWheelSpeed( );
        
        /*! \brief Start the PID controller in a separate thread
         */
        void StartPID( );
        
        /*! \brief Friend function that is spawned in a new thread in order to run the PID controller.  It is called 
         *  internally by the CommandWheelSpeed() function.
         *  PID controller to set momentum wheel angular velocity.  Runs in a separate thread created by
         *       by the CommandWheelSpeed() function.  THis function is a friend to the PhyicalMomentumWheel class.
         *       Once created it will run indefinitely until either a new speed is requested or the end of the program
         *       is reached.
         *
         *       This controller has gain scheduling based on the magnitude of the velocity change desired.  It
         *       also places limits on large changes in torque and limits the upper value of torque in order
         *       to protect the motor and its components.
         *      
         *  @param arg the argument is set to be a "this" pointer cast as a void pointer 
         */
        friend void* RunPIDController(void* arg);
        
        /*! \brief End the PID Control Thread (will be called by the deconstructor)
         */
        void StopPID( );

        
        /*! \brief Old-style Command a torque to the wheel
         *  @param wheelTorque double representing the desired torque output in N-m 
         */
        void CommandSystemTorque( double wheelTorque );

        /////////////////////////////////////////////////
        // Mutators
        ////////////////////////////////////////////////
    
        /*! \brief Set the maximun and minimum torques to be applied to the wheel
         *  @param minWheelTorque The minimum torque that should be applied to the wheel in N-m
         *  @param maxWheelTorque The maaximum torque that shoul be applied to the wheel in N-m 
         */
        void SetWheelTorqueLimits( const double& minWheelTorque, const double& maxWheelTorque );

        /*! \brief Set the maximum change between the current and desired torque values
         *  @param maxTorqueStep The maximun allowed cahnge in torque in N-m 
         */
        void SetMaxTorqueStep( const double maxTorqueStep );

        /*! \brief Set the address of the wheel in the daisy chain
         *  @param WheelAddress integer representing the address of the wheel 
         */
        void SetWheelAddress( int WheelAddress );

        /*! \brief Set a daisy chain number to distinguish between two or more daisy chains 
         *  @param DaisyChainNumber 
         */
        void SetDaisyChainNumber( int DaisyChainNumber );

        /*! \brief Set the port to which the wheel's chain is connected
         * @param DaisyChainPort The string perpresenting the port loaction, S0 for serial 1, USB1 for usb port 2, etc 
         */
        void SetDaisyChainPort( string DaisyChainPort );
    
        /*! \brief Set the bais speed of the wheel for a momentum wheel. 
         *  @param BiasSpeed [rad/s]
         */
        void SetBiasSpeed( double BiasSpeed );

        /*! \brief Set the axial inertia of the wheel 
         *  @param _inertia the spin axis ineria in kg-m^2
         */
        void SetAxialInertia( double _inertia );
        
        /*! \brief Set the momentum wheel string name
         *  @param _wheelName A string naming the wheel
         */
        void SetWheelName( string _wheelName );

        /*! \brief Set the momentum wheel speed coefficients. The wheel speed coefficients are determined from the characterization of each wheel motor.
         *  @param _Coefficients1 a 4x1 vector of constants
         *  @param _Coefficients2 a 3x1 vector of constants
         *  @param _Coefficients3 a 3x1 vector of constants
         *  @param _Coefficients4 a 4x1 vector of constants
         */
        void SetWheelSpeedCoefficients( Vector _Coefficients1, Vector _Coefficients2, Vector _Coefficients3, Vector _Coefficients4 );

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

        /*! /brief Returns the most current speed reported by the wheel, a double in rad/sec 
         *  @param Speed double set to the current wheel speed
         *  @param measTime double set to the number of seconds since unix epoch when the velocity waas measured
         */
        void GetWheelSpeed( double& Speed, double& measTime );

        /*! \brief Returns a double representing the current torque to the wheel in N-m 
         */
        double QueryWheelTorque( );

        /*! \brief Get wheel inertia
         *  @param _inertia is the axial inertia kg-m^2
         */
        void GetAxialInertia( double& _inertia );       
        
        /*! \brief Returns a type double of wheel inertia kg-m^2*/
        double GetAxialInertia( );      
        
        /*! \brief Returns a type string of the wheel name */
        string GetWheelName( ); 
        
        /*! \brief Sets the parameters to the min and max torque limits
         *  @param minWheelTorque Set to the minimum allowed wheel torque in N-m
         *  @param maxWheelTorque Set to the maximum allowed wheel torque in N-m 
        */
        void GetWheelTorqueLimits( double& minWheelTorque, double& maxWheelTorque );

        /*! \brief Sets the parameter maxTorqueStep to the maximum allowed torque step in N-m 
         *  @param maxTorqueStep 
         */
        void GetMaxTorqueStep( double& maxTorqueStep );
        
        /*! \brief Get the Wheel file descriptor 
         */
        int GetWheelFD( );
        
        /*! \brief Get the amps reported by the motor 
         */
        double GetAmps( );

        /*! \brief Get the voltage the motor 
         */
        double GetVolts( );

protected: 
    
   
private:
        /*! \brief Member double that holds the current angular wheel speed in rad/sec */
        double m_CurrentWheelSpeed;
        
        /*! \brief Memeber double holds the time of velcotiy measurement in seconds from unix epoch */
        double m_VelocityTime;

        /*! \brief Member double that holds the desired angular wheel speed in rad/sec  */
        double m_DesiredWheelSpeed;
    
        /*! \brief Member double that holds the maximum output torque in N-m */
        double m_MaxWheelTorque;

        /*! \brief Member double that holds the minimum output torque in N-m */
        double m_MinWheelTorque;

        /*! \brief Member double that holds the current ouput torque in N-m */
        double m_CurrentWheelTorque;

        /*! \brief Desired wheel Torque*/
        double m_DesiredWheelTorque;

        /*! \brief Member double that holds the axial Inertia in kg-m^2*/
        double m_AxialInertia;

        /*! \brief Member integer Bias speed set flag */
        int m_BiasFlag;
        
        /*! \brief Momentum Wheel bias speed */
        double m_BiasSpeed;

        /*! \brief Member integer that holds the address of the wheel on its daisy chain */
        int m_WheelAddress;

        /*! \brief Member integer that holds which daisy chain the wheel is on */
        int m_DaisyChainNumber;

        /*! \brief Member integer that holds the file descriptor for all wheels */
        int m_fd;

        /*! \brief Member string that hold which port the wheel is connected to */
        string m_DaisyChainPort;

        /*! \brief Member double that hold minimum volts of the reaction wheel motor */
        double m_MinVolts;

        /*! \brief Member double that hold maximum current of the reaction wheel motor */
        double m_MaxAmps;

        /*! \brief Member double that hold maximum current of the reaction wheel motor */
        string m_WheelName;
        
        /////////////////////////////
        // Threads
        ////////////////////////////

        /*! \brief Member pthread id for splitting off PID control */
        pthread_t m_PIDThreadID;
        
        /*! \brief Member pthread for splitting off speed queries */
        pthread_t m_SpeedThreadID;

        /*! \brief Member pthread for Torque Controller */
        pthread_t m_TorqueThreadID;
    
        /*! \brief Memeber mutex lock variable*/
        pthread_mutex_t m_Mutex;

        int m_exitConditionSpeed;
        
        int m_exitConditionTorque;
        
        /////////////////////////
        //PID control holders
        ////////////////////////
        /*! \brief Member double that holds the last angular velocity of the wheel, used in PID controller */
        double LAST_VEL;

        /*! \brief Member double that holds the last applied wheel torque, used in PID controller */
        double LAST_TOR;

        /*! \brief Member double that holds the last velocity error, used by the PID controller */
        double LAST_ERR;
        
        /*! \brief Member double that holds the last voltage, used by PID controller */
        double LAST_VOLT;
    
        /*! \brief Member double that holds the largest permissable torque change in N-m */
        double m_MaxTorqueStep;
        
        /////////////////////////////
        // Torque Controller Holders
        ////////////////////////////

        /*! \brief Member integer Torque Control flag, used in Torque Controller */
        int m_TorqueControlFlag;
        
        /*! \brief Delta Time to produce Desired Control Torque, used in Torque Controller [s] */    
        double m_TorqueControllerDeltaTime;

        /*! \brief Proportional Torque Gain (to tune function to supply torques closer to actual N-m) */
        double m_TorqueControllerTorqueGain;
        
        /*! \brief Negative Time Gain */
        double m_TorqueControllerNegativeTimeGain;
        
        /*! \brief Steady State Gain [rad/s]*/
        double m_TorqueControllerSteadyStateGain;

        /*! \brief Member integer Torque Control Wheel Speed flag, used in Torque Controller */
        int m_TorqueControllerDesiredWheelSpeedFlag;

        /////////////////////////////
        // Wheel Speed Coefficients
        ////////////////////////////
        
        Vector m_Coefficients1;
        Vector m_Coefficients2;
        Vector m_Coefficients3;
        Vector m_Coefficients4;


};

#endif 
// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: PhysicalMomentumWheel.h,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/05/29 18:57:41  jayhawk_hokie
*       *** empty log message ***
*       
*       Revision 1.32  2007/04/09 14:01:31  jayhawk_hokie
*       Added capability in Torque Controller to reduce wheel speed.
*       
*       Revision 1.31  2007/03/27 19:07:16  jayhawk_hokie
*       Updated Momentum Wheels.
*       
*       Revision 1.30  2007/02/06 20:56:23  jayhawk_hokie
*       Added SetAxialInertia function.
*       
*       Revision 1.29  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.28  2004/05/11 20:18:09  bstreetman
*       Vast improvements to torque controller
*       
*       Revision 1.27  2004/04/19 15:00:41  bstreetman
*       Fixed a config and a shutdown bug
*       
*       Revision 1.26  2004/03/18 18:08:17  bstreetman
*       Further Fixes on the torque controller
*       
*       Revision 1.25  2004/03/15 21:58:31  bstreetman
*       Fixed a multible multi-thread issue.
*       
*       Revision 1.24  2004/03/04 23:13:09  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.23  2003/12/16 17:20:26  bstreetman
*       A bit o' fall cleaning.  Just look at how pretty it is.
*       
*       Revision 1.22  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.21  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.20  2003/09/15 21:44:03  bstreetman
*       Removed hardcoded parameters, ready for pure config file use
*       
*       Revision 1.19  2003/08/18 21:30:20  bstreetman
*       PID controller now functions properly in multiple threads. RunPIDController() now a non-member friend funciton
*       
*       Revision 1.17  2003/08/04 21:25:23  bstreetman
*       Now with doxygen-style comments
*       
*       Revision 1.16  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.15  2003/07/28 21:08:00  bstreetman
*       Now with PID controller, gains not perfect yet.  RUns fine, just slow getting the wheels up to speed
*       
*       Revision 1.14  2003/07/21 20:15:30  bstreetman
*       Updates
*       
*       Revision 1.13  2003/07/21 18:28:28  cskelton
*       Current copies
*       
*       Revision 1.12  2003/07/09 20:24:33  cskelton
*       Corrected Initialization and Deconstruction
*       
*       Revision 1.11  2003/07/07 19:34:08  cskelton
*       Removed virtual from functions
*       
*       Revision 1.10  2003/07/04 16:01:31  nicmcp
*       took out the Axial inertia and added it to Momentumwheel.h
*       
*       Revision 1.9  2003/07/02 22:02:12  bstreetman
*       Improve functionality and made to work with the c hardware code.
*       
*       Revision 1.8  2003/07/01 19:56:09  cskelton
*       Added Hardware software support
*       
*       Revision 1.7  2003/07/01 19:19:43  cskelton
*       Debugged to compile
*       
*       Revision 1.6  2003/07/01 16:00:23  bstreetman
*       Added CommandWheelSpeed function
*       
*       Revision 1.5  2003/06/30 21:04:57  cskelton
*       INserted actual functions
*       
*       Revision 1.4  2003/06/30 18:47:12  cskelton
*       Updated Functions
*       
*       Revision 1.3  2003/06/30 15:33:21  cskelton
*       Now PMW dervived from Actuators
*       
*       Revision 1.2  2003/06/26 20:53:28  bstreetman
*       Changed inputs of CommandWheelToque func
*       
*       Revision 1.1  2003/06/19 21:31:55  simpliciter
*       Initial submission, upgrades from Andy's generic code.
*       
*       Revision 1.1  2003/06/13 15:24:33  simpliciter
*       Initial submission.
*       
*
******************************************************************************/

---