PhysicalMomentumWheel.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file PhysicalMomentumWheel.cpp
*  \brief Implementation of the PhysicalMomentumWheel class
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.44 $
*  \date    $Date: 2007/08/31 15:48:03 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "PhysicalMomentumWheel.h"

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

PhysicalMomentumWheel::PhysicalMomentumWheel( )
{
        pthread_mutex_init( &m_Mutex, NULL );


        m_fd = -1;
        m_BiasFlag = 0;
        m_CurrentWheelSpeed = 0.0; // [rad/s]

        m_MinVolts = 15; // [V]
        m_MaxAmps  = 24; // [A]

        //With WheelHistory far from implementation, members vars are used to store last state info

        // PID Controller Parameters
        LAST_VEL = 0.0;
        LAST_TOR = 0.0;
        LAST_ERR = 0.0;
        LAST_VOLT = 25.0; // [V]

        // Torque Controller Parameters
        m_TorqueControlFlag = 1;
        m_TorqueControllerDeltaTime = 0.4; // [s]
        m_TorqueControllerTorqueGain = 1; 
        m_TorqueControllerNegativeTimeGain = 0.5; // [s]
        m_TorqueControllerSteadyStateGain = 1.0; // [rad/s]
        m_TorqueControllerDesiredWheelSpeedFlag = 0;
        
        m_exitConditionTorque = 1;
        
        //Set SmartMotor 3430 physical limits
        m_MaxWheelTorque = 1023;

        m_MinWheelTorque = -m_MaxWheelTorque;
        m_MaxTorqueStep = 700;

        // Wheel Speed Coefficients
        m_Coefficients1.initialize(4);
        m_Coefficients2.initialize(3);
        m_Coefficients3.initialize(3);
        m_Coefficients4.initialize(4);
        
        m_exitConditionSpeed = 1;

}

PhysicalMomentumWheel::~PhysicalMomentumWheel( )
{

        Deinitialize();

}

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


int PhysicalMomentumWheel::Initialize( )
{
        //pthread_mutex_lock( &m_Mutex );
        
        char *stringPort;
        stringPort = (char *)malloc(80);
        
        if ( m_fd < 0 ) // g_fd has NOT been set yet, then init the daisychain
        {
                sprintf(stringPort,"%s",m_DaisyChainPort.c_str());
                
                init_serial( stringPort, B9600, &m_fd );

                Reset( ); // Reset Wheel Motor settings

                ChangeBaud( ); // Change Baud Rate      
        
                close( m_fd );
                
                init_serial( stringPort, B19200, &m_fd );
                
                EchoOff( ); // Set ECHO function to off 
         
                EnableTorqueMode( ); // Enable torque mode for the wheel motor
        } 

        //pthread_mutex_unlock( &m_Mutex );

        if( m_BiasFlag )
        {
                m_DesiredWheelSpeed = m_BiasSpeed ;
                CommandWheelSpeed( );
        }
          
        // Start wheel Query
        cerr << "Start Speed Query for Wheel" << endl;
        StartSpeedQuery( );
        usleep( 500000 ); // necessary for wheels to get first speed.

        cerr << "Wheel Initialized: " << m_WheelName << endl;

        return( 0 );
}


int PhysicalMomentumWheel::Deinitialize( ) 
{
        cerr << "\n\nDEINITIALIZING WHEEL: " << m_WheelName << endl;

        
        if ( !m_TorqueControlFlag )
        {
                cerr << "Stop Torque Controller" << endl;
                StopTorqueController( );
        }

        cerr << "Stop Wheel " << endl;
        Stop( );

        cerr << "Stop Speed Query" << endl;
        StopSpeedQuery( );

        cerr << "Deallocating memory from mutex" << endl;
        pthread_mutex_destroy( &m_Mutex );

        cerr << "Serial Port Closing:\n";
        close( m_fd );
        return( 0 );
}


int PhysicalMomentumWheel::Reset( )
{
        char *command;
        command = new char[80];
        sprintf( command, "Z\n\n" );
        pthread_mutex_lock(&m_Mutex);
                say( m_fd, command );
                cerr << "\nRESET MOMENTUM WHEEL SETTINGS for wheel: " << m_WheelName << endl;
        pthread_mutex_unlock(&m_Mutex);
        // pause 1.5 seconds
        usleep(1500000);
        delete command;
        return( 0 );
}

int PhysicalMomentumWheel::ChangeBaud( )
{
        char *posString = new char[80];
        char *command;
        command = new char[80];
        sprintf( command, "BAUD19200\n\n" );
        pthread_mutex_lock(&m_Mutex);
                say( m_fd, command );
                cerr << "RESET BAUD to 19200: " << m_WheelName << endl;
                hear( m_fd, posString, 15, 0, 50000, '\r' );
        pthread_mutex_unlock(&m_Mutex);
        // pause 1.5 seconds
        usleep(1500000);
        delete command;
        delete posString;
        return( 0 );
}

int PhysicalMomentumWheel::EchoOn( )
{
        char *command;
        command = new char[80];
        sprintf( command, "ECHO\n\n" );
        pthread_mutex_lock(&m_Mutex);
                say( m_fd, command );
                cerr << "SET ECHO ON for wheel: " << m_WheelName  << endl;
        pthread_mutex_unlock(&m_Mutex);
        // pause 1.5 seconds
        usleep(1500000);
        delete command;
        return( 0 );
}

int PhysicalMomentumWheel::EchoOff( )
{
        char *command;
        command = new char[80];
        sprintf( command, "ECHO_OFF\n\n" );
        pthread_mutex_lock(&m_Mutex);
                say( m_fd, command );
                cerr << "SET ECHO OFF for wheel: " << m_WheelName  << endl;
        pthread_mutex_unlock(&m_Mutex);
        // pause 1.5 seconds
        usleep(1500000);
        delete command;
        return( 0 );
}

int PhysicalMomentumWheel::EnableTorqueMode( )
{
        char *command;
        command = new char[80];
        sprintf( command, "MT\n\n" );
        pthread_mutex_lock(&m_Mutex);
                say( m_fd, command );
                cerr << "ENABLE TORQUE MODE for wheel: " << m_WheelName  << endl;
        pthread_mutex_unlock(&m_Mutex);
        // pause 1.5 seconds
        usleep(1500000);
        delete command;
        return( 0 );
}

int PhysicalMomentumWheel::TorqueCommand( double _power )
{
        static char *torqueCommand = new char[80];
        sprintf( torqueCommand, "T%f\n\n", _power );
        pthread_mutex_lock(&m_Mutex);
                say(m_fd, torqueCommand);
        pthread_mutex_unlock(&m_Mutex);
        return( 0 );
}


int PhysicalMomentumWheel::Stop( )
{
        pthread_mutex_unlock(&m_Mutex); // encase mutex lock has been activated in another function
        Reset( );
        return( 0 );
}

void PhysicalMomentumWheel::StartSpeedQuery( )
{
        pthread_create( &m_SpeedThreadID, NULL, QueryWheelSpeed, (void*) this );
}

void* QueryWheelSpeed( void* arg )
{
        PhysicalMomentumWheel *wheel = (PhysicalMomentumWheel*) arg;
        double pos1 = 0.0;
        double pos2 = 0.0;
        double timeStamp1 = 0.0;
        double timeStamp2 = 0.0;
        char *posString = new char[80];
        char *posString2 = new char[80];
        double velocity = 0.0 ;
        int readdelay = 100000; // 1000000
        char *RPCommand = new char[80];

        
        // First Point
        
        //clear serial port buffer
        //char* clear = new char[80];
        //begin commanding and reading
        sprintf(RPCommand, "RP\n");

        // Lock Reaction Wheel Parameters
        pthread_mutex_lock(&wheel->m_Mutex);

                //while( !hear( wheel->m_fd, clear, 79, 0, 3333, 'c' ));

                double timeA = Now( );
                hear( wheel->m_fd, posString, 15, 0, 50000, '\r' );
                double timeB = Now( );
                timeStamp1 = (timeB + timeA)/2;
                //say(wheel->m_fd, RPCommand);
                //timeStamp1 = Now();
                //hear( wheel->m_fd, posString, 15, 0, 50000, '\r' );
        // Unlock Reaction Wheel Parameters
        pthread_mutex_unlock(&wheel->m_Mutex);
        pos1 = atoi(posString);

        int exit = 1;   

        while( exit )
        {
                pthread_testcancel();

                //clear serial port buffer
                //char* clear = new char[80];
                //begin commanding and reading
                sprintf(RPCommand, "RP\n");
                
                usleep(readdelay);
/*              
        // Lock Reaction Wheel Parameters
        pthread_mutex_lock(&wheel->m_Mutex);

                //while( !hear( wheel->m_fd, clear, 79, 0, 3333, 'c' ));

                say(wheel->m_fd, RPCommand);
                
                double timeA = Now( );
                hear( wheel->m_fd, posString, 15, 0, 50000, '\r' );
                double timeB = Now( );
                timeStamp1 = (timeB + timeA)/2;
                //timeStamp1 = timeB;
        // Unlock Reaction Wheel Parameters
        //pthread_mutex_unlock(&wheel->m_Mutex);
        
                pos1 = atoi(posString);

        pthread_testcancel( );

        usleep( 100000 );
*/              
                //position2
                // Lock Reaction Wheel Parameters
                pthread_mutex_lock(&wheel->m_Mutex);

                        say(wheel->m_fd, RPCommand);
                
                        double timeC = Now( );
                        hear(wheel->m_fd, posString2, 15, 0, 50000, '\r');
                        double timeD = Now( );
                        timeStamp2 = (timeD + timeC )/2;
                        //timeStamp2 =  timeC;
                // Unlock Reaction Wheel Parameters
                pthread_mutex_unlock(&wheel->m_Mutex);
        
                pos2 = atoi(posString2);
                
                // Negative sign for correct rotation direction. Right Hand Rule -> along shaft away from motor. 
                velocity = -( (pos2 - pos1) /( timeStamp2 - timeStamp1 ))*2*3.14159/4000.0; // rad/sec => 2pi rad/4000 ct
                //velocity = -( (pos2 - pos1) /( 20000 ))*2*3.14159/4000.0; // rad/sec => 2pi rad/4000 ct

                //timeStamp2 = Now( );

                pthread_mutex_lock(&wheel->m_Mutex);
                        wheel->m_CurrentWheelSpeed = velocity; // (rad/s)
                        wheel->m_VelocityTime = timeStamp2; // (s)      
                pthread_mutex_unlock(&wheel->m_Mutex);
                
                // Update to use for next measurement
                pos1 = pos2;
                timeStamp1 = timeStamp2;                
                
                pthread_testcancel( );
                
                // Lock Reaction Wheel Parameters
                pthread_mutex_lock(&wheel->m_Mutex);
                        exit = wheel->m_exitConditionSpeed;
                // Unlock Reaction Wheel Parameters
                pthread_mutex_unlock(&wheel->m_Mutex);

        }

        return arg;
}

void PhysicalMomentumWheel::StopSpeedQuery( )
{
        pthread_mutex_lock( &m_Mutex );
                m_exitConditionSpeed = 0;
        pthread_mutex_unlock( &m_Mutex );
                
        pthread_join( m_SpeedThreadID, NULL );
}


void PhysicalMomentumWheel::CommandWheelTorque( double _wheelTorque )
{
        if( m_TorqueControlFlag )
        {
                StartTorqueController( );
                m_TorqueControlFlag = 0;
        }
        // Lock Reaction Wheel Parameters
        pthread_mutex_lock(&m_Mutex);
                m_DesiredWheelTorque = _wheelTorque;
        // Unlock Reaction Wheel Parameters
        pthread_mutex_unlock(&m_Mutex);

}

void PhysicalMomentumWheel::StartTorqueController()
{
        //pthread_create(&m_TorqueThreadID, NULL, RunTorqueController, (void*) this);
        pthread_create(&m_TorqueThreadID, NULL, RunTorqueControllerRev, (void*) this);
}


void* RunTorqueControllerRev( void* arg )
{
        // Create PhysicalMomentumWheel Object and get parameters from arg
        PhysicalMomentumWheel *reactionWheel = (PhysicalMomentumWheel*) arg;

        int CommandWheelSpeedFlag = 0;
        
        // Determine Steady State Coefficient [~]
        /* equation not good enough
        double Constant  = -0.000000000000006*pow( reactionWheel->m_TorqueControllerDeltaTime, 10 )
                        + 0.000000000002074*pow( reactionWheel->m_TorqueControllerDeltaTime , 9 ) 
                        - 0.0000000002887*pow( reactionWheel->m_TorqueControllerDeltaTime , 8 ) 
                        + 0.00000002271*pow( reactionWheel->m_TorqueControllerDeltaTime , 7 ) 
                        - 0.000001111*pow( reactionWheel->m_TorqueControllerDeltaTime , 6 ) 
                        + 0.000003512*pow( reactionWheel->m_TorqueControllerDeltaTime , 5 ) 
                        - 0.0007216*pow(  reactionWheel->m_TorqueControllerDeltaTime, 4 ) 
                        + 0.009515*pow(  reactionWheel->m_TorqueControllerDeltaTime, 3 ) 
                        - 0.07825*pow(  reactionWheel->m_TorqueControllerDeltaTime, 2 ) 
                        + 0.3839* reactionWheel->m_TorqueControllerDeltaTime + 0.02609;
        */
        double Constant = 0;
        double ConstantPositive = 0.109; // for increasing wheel speed.
        //double ConstantNegative = 0.05; // didn't work! for deacreasing wheel speed. Derived from looking at steady state condition.
        double ConstantNegative = 0.109; // for deacreasing wheel speed. set the same as positive value.
        //Constant = 0.04406469;
        double time1=0;
        double time2=0;
        double desiredTorque=0.0;

        double steadyStateWheelSpeed = 0.0;

        double reduceWheelSpeedTimeGain = 0.0; 
        //double reduceWheelSpeedGain = 0.15; 
        double reduceWheelSpeedGain = 0.25; 
        double delayTime = 0.0;
        int condition = 0;

        int exit = 1;

        // The torque controller will run until the thread is terminated with a deinitialization call.
        while( exit )
        {
                        
                time1 = Now( );
                
                reduceWheelSpeedTimeGain = 1; 
                
                // Lock Reaction Wheel Parameters
                pthread_mutex_lock( &reactionWheel->m_Mutex );
                        // Determine Final Wheel Speed [rad/s].  Note negative will spin the reaction wheels in the opposite direction to provide the correct +/- torque.
                        
                        desiredTorque = reactionWheel->m_DesiredWheelTorque; 
                        
                        if ( reactionWheel->m_DesiredWheelTorque )
                        {
                                double finalWheelSpeed = reactionWheel->m_CurrentWheelSpeed 
                                        + ( reactionWheel->m_TorqueControllerTorqueGain*(-reactionWheel->m_DesiredWheelTorque) )
                                        * reactionWheel->m_TorqueControllerDeltaTime/reactionWheel->m_AxialInertia;  
                                // Determine Steady State Wheel Speed [rad/s]
                                // This might produce jumpy results.
                                if ( finalWheelSpeed < reactionWheel->m_CurrentWheelSpeed && finalWheelSpeed >= 0 && reactionWheel->m_CurrentWheelSpeed >=0 )
                                {
                                        condition = 1;
                                }
                                else if ( finalWheelSpeed > reactionWheel->m_CurrentWheelSpeed && finalWheelSpeed <= 0 && reactionWheel->m_CurrentWheelSpeed <= 0 )
                                {
                                        condition = 1;
                                }
                                else
                                {
                                        condition = 0;
                                }
                                if ( condition )
                                {
                                        //cerr << "Decrease Wheel Speed: " << finalWheelSpeed << " Current= " << reactionWheel->m_CurrentWheelSpeed << endl; 
                                        Constant = ConstantNegative; // This should act as a brake for the wheel.
                                        //steadyStateWheelSpeed = reduceWheelSpeedGain*reactionWheel->m_TorqueControllerSteadyStateGain
                                        //      * ( finalWheelSpeed + ( Constant - 1 )*reactionWheel->m_CurrentWheelSpeed )/Constant; 
                                        steadyStateWheelSpeed = reduceWheelSpeedGain*reactionWheel->m_TorqueControllerSteadyStateGain
                                                * ( finalWheelSpeed - reactionWheel->m_CurrentWheelSpeed )/Constant; 
                                        reduceWheelSpeedTimeGain = 0.2; // experimentally determined 
                                }
                                else
                                {
                                        Constant = ConstantPositive;
                                        steadyStateWheelSpeed = reactionWheel->m_TorqueControllerSteadyStateGain
                                                * ( finalWheelSpeed + ( Constant - 1 )*reactionWheel->m_CurrentWheelSpeed )/Constant; 
                                }
                                // Set Reaction Wheel Speed.
                                reactionWheel->m_DesiredWheelSpeed = steadyStateWheelSpeed;
                                // Reset Control Torque to Zero
                                //desiredTorque = reactionWheel->m_DesiredWheelTorque; 
                                reactionWheel->m_DesiredWheelTorque = 0; 
                                reactionWheel->m_TorqueControllerDesiredWheelSpeedFlag = 1; 
                                CommandWheelSpeedFlag = 1;
                        }
                        else
                        {
                                if( reactionWheel->m_TorqueControllerDesiredWheelSpeedFlag )
                                {
                                        reactionWheel->m_DesiredWheelSpeed = reactionWheel->m_CurrentWheelSpeed;
                                        reactionWheel->m_TorqueControllerDesiredWheelSpeedFlag = 0;
                                        CommandWheelSpeedFlag = 1;
                                }
                                else
                                {
                                        CommandWheelSpeedFlag = 0;
                                }
                        }

                        delayTime = reactionWheel->m_TorqueControllerDeltaTime*1000000*reduceWheelSpeedTimeGain;

                // Unlock Reaction Wheel Parameters
                pthread_mutex_unlock( &reactionWheel->m_Mutex );
                
                
                // Lock Reaction Wheel Parameters
                pthread_mutex_lock( &reactionWheel->m_Mutex );
                        exit = reactionWheel->m_exitConditionTorque;
                // Unlock Reaction Wheel Parameters
                pthread_mutex_unlock( &reactionWheel->m_Mutex );
                
                time2 = time1;
                //cerr << "delta Time = " << time1-time2 << endl;
                
                if( CommandWheelSpeedFlag )
                {
                        // Determine Power Setting and Send to Reaction Wheel Motor
                        reactionWheel->CommandWheelSpeed( );
                }
                
                
                usleep( int( delayTime ) );
        } 

        return arg;
}

/*
        double wheelAxialInertia; // wheel axial inertia (kg/m^2)
        wheel->GetAxialInertia( wheelAxialInertia );

        double initialWheelSpeed = 0; // (rad/s)

        double desiredWheelSpeed = 0; // (rad/s)

        double steadyStateWheelSpeed = 0; // (rad/s)

        double augmentedSteadyStateWheelSpeed = 0; // (rad/s)

        double desiredTorque = 0; // (N-m)

        double deltaTime = 1.0;//0.5; // (s)


        int negflag = 1;

        double torqueGain = 1;  // proportional gain (to tune function to supply torques closer to actual N-m)
        
        // Proportional Torque Gain (to tune function to supply torques closer to actual N-m)
        double m_TorqueControllerTorqueGain;

        // Negative Time Gain
        double m_TorqueControllerNegativeTimeGain;  


        double negativeTimeGain = 0.5;  //negative time gain

        double augmentationGain = 1.12; //positive augmentation gain

        double steadyStateGain = 7; //positive steady state (rad/s)

        double minimumTorque = 0.003; //torque threshold

        while( 1 ) // endless loop
        {
                pthread_testcancel();

                pthread_mutex_lock( &wheel->m_Mutex );

                        desiredTorque = torqueGain * wheel->m_DesiredWheelTorque; // from controller

                        initialWheelSpeed = - wheel->m_CurrentWheelSpeed;

                        // not smart method for limiting wheel speed!
                        if( fabs( initialWheelSpeed ) > 200 ) // float absolute of w1, (rad/s) (~1900 rad/s)
                        {
                                initialWheelSpeed = wheel->LAST_VEL; // (rad/s)
                        }
                
                pthread_mutex_unlock( &wheel->m_Mutex );
                
                pthread_testcancel();
                
                desiredWheelSpeed = initialWheelSpeed + ( desiredTorque ) * deltaTime / ( wheelAxialInertia );

                steadyStateWheelSpeed = ( desiredWheelSpeed + ( Constant - 1 ) * initialWheelSpeed ) / Constant;

                // Command Logic for wheel angular rates.
                if( fabs( desiredTorque ) < minimumTorque ) // don't do anything
                {
                        usleep( (int) ( deltaTime*1000000 ) );
                } 
                else if(negflag)
                {
                        augmentedSteadyStateWheelSpeed = steadyStateWheelSpeed*augmentationGain + steadyStateWheelSpeed*steadyStateGain/fabs( steadyStateWheelSpeed );
                        wheel->SetWheelSpeed( augmentedSteadyStateWheelSpeed );
                        wheel->CommandWheelSpeed( );
                        usleep( (int) (deltaTime*1000000) );
                } 
                else // what is this?
                {
                        usleep((int) ( deltaTime*1000000 - deltaTime*1000000*fabs( desiredTorque )*negativeTimeGain ) );
                }
                
                negflag = 1;

                pthread_mutex_lock(&wheel->m_Mutex);

                        wheel->LAST_VEL = initialWheelSpeed;

                pthread_mutex_unlock( &wheel->m_Mutex );

                pthread_testcancel();
        }
*/
        //return ;
//}


/*! \brief Determine reaction wheel speed to provide the desired torque requested from controller.
  * 
  * @param void* arg
  */
void* RunTorqueController(void* arg)
{
        PhysicalMomentumWheel *wheel = (PhysicalMomentumWheel*) arg;

        double wheelAxialInertia; // wheel axial inertia (kg/m^2)
        wheel->GetAxialInertia( wheelAxialInertia );

        double w1;

        double w2;

        double wss;

        double desiredTorque = 0;

        double deltat = 0.5;

        double C  = 0.000001906374*pow(deltat,5) - 0.00012550397*pow(deltat,4) + 0.0032678612*pow(deltat,3) 
                        - 0.043053012550397*pow(deltat,4) + 0.0032678612*pow(deltat,3) - 0.0957*pow(deltat,2) 
                        + 0.3024019105*deltat + 0.00429944309;

        int negflag = 1;

        double k = 1;  // proportional gain (to tune function to supply torques closer to actual N-m)

        double knt = 0.5;  //negative time gain

        double kpa = 1.12; //positive augmentation gain

        double kps = 7; //positive steady state

        double minT = 0.03; //torque threshold

        while( 1 ) // endless loop
        {
                pthread_testcancel();

                pthread_mutex_lock( &wheel->m_Mutex );

                desiredTorque = k * wheel->m_DesiredWheelTorque; // from controller

                // why negative?
                w1 = - wheel->m_CurrentWheelSpeed;

                cout << "w1: " << w1 << endl;

                if( fabs( w1 ) > 200 ) // float absolute of w1, (rad/s)
                {
                        w1 = wheel->LAST_VEL; // (rad/s)
                }
                
                cout << "w1 after check: " << w1 << endl;

                pthread_mutex_unlock( &wheel->m_Mutex );
                
                //k = fabs(desTor)/.3; // proportional gain (to tune function to supply torques closer to actual N-m)
                //desTor = k*desTor;
                
                // crude integration
                w2 = w1 + ( desiredTorque ) * deltat / ( wheelAxialInertia );

                cout << "w2: " << w2 << endl;
        
                wss = ( w2 + ( C - 1 ) * w1 ) / C;

                cout << "wss: " << wss << endl;
                
        //      pthread_testcancel();

        /*
                if(fabs(wss)<fabs(w1) && wss*w1 > 0){
                        deltat = -0.0844/(-0.1744+(desTor/(I*-(w1))));
                        w2 = w1 + (desTor)*deltat/I;
                        //sleept = 310000;
                        wheel->CommandSystemTorque(-w1/fabs(w1)*0.0018);
                }
        */

        /*
                if( fabs( w2 ) < fabs( w1 ) && fabs( desiredTorque ) > ( k*minT ) )
                {
                        wheel->CommandSystemTorque( -w1 / fabs(w1) * 0.0018 );
                        usleep( (int) ( deltat * 1000000 * fabs(desiredTorque) * knt ) );
                        negflag = 0;

                        cout << " Inside CommandSystemTorque loop " << endl;

                        //of << wheel->m_DaisyChainPort[3] << " " << desTor << " " << wss << " " << w2 << " " << w1 << endl;
                }
        */
        //      pthread_testcancel();

                //of << wheel->m_DaisyChainPort[3] << " " << desTor << " " << wss << " " << w2 << " " << w1 << " " << negflag << endl;

                // what the hell?
                //wheel->CommandWheelSpeed( wss );//+20*wss/fabs(wss));
                
                //cout << "command wss " << endl;
                
                // Command Logic for wheel angular rates.
                if( fabs( desiredTorque ) < minT) // don't do anything
                {
                        usleep((int) (deltat*1000000));
                } 
                else if(negflag)
                {
                        wheel->SetWheelSpeed( wss*kpa + kps*wss/fabs(wss) );
                        wheel->CommandWheelSpeed( );
                        usleep((int) (deltat*1000000));
                } 
                else // what is this?
                {
                        usleep((int) (deltat*1000000 - deltat*1000000*fabs(desiredTorque)*knt));
                }
                
                // what the hell part 2
                //wheel->CommandWheelSpeed(w2);
                //cout << "command w2 " << endl;

                negflag = 1;

                pthread_mutex_lock(&wheel->m_Mutex);

                // doesn't seem to make sense
                wheel->LAST_VEL = w1;

                pthread_mutex_unlock(&wheel->m_Mutex);

                pthread_testcancel();
        }
        return NULL;
}

void PhysicalMomentumWheel::StopTorqueController( )
{
        pthread_mutex_lock( &m_Mutex );
                m_exitConditionTorque = 0;
        pthread_mutex_unlock( &m_Mutex );
        
        pthread_join( m_TorqueThreadID, NULL );
}

void PhysicalMomentumWheel::SetWheelSpeed( double desiredWheelSpeed )
{
        pthread_mutex_lock( &m_Mutex );
                m_DesiredWheelSpeed = desiredWheelSpeed;
        pthread_mutex_unlock( &m_Mutex );
}

void PhysicalMomentumWheel::CommandWheelSpeed( )
{

        // Before applying wheel command check Voltage and Amps
        double volts;
        double amps;
        
        volts = GetVolts( );
        amps = GetAmps( );
        
        if( fabs(volts) < m_MinVolts )
        {
                cerr << "WARNING: " << m_WheelName << " Voltage is LOW! Voltage = " << volts << " .... Rechecking Voltage " << endl;
                volts = GetVolts( );
                if( fabs( volts ) < m_MinVolts )
                {
                        cerr << "Warning 2nd Check: Voltage Too LOW! Voltage = "<< volts << " .... No Command Sent to Motor " << m_WheelName  << endl;
                        return;
                }
        }
        else if( fabs( amps ) > m_MaxAmps )
        {
                cerr << "WARNING: " << m_WheelName << " Current is High! Amps = " << amps << " .... Rechecking Current " << endl;
                amps = GetAmps( );
                if( fabs(amps) > m_MaxAmps )
                {
                        cerr << "Warning 2nd Check: Current Too HIGH! Amps = "<< amps << " .... No Command Sent to Motor " << m_WheelName  << endl;
                        return;
                }
        }
        else
        { }

        // values derived for Z wheel motor.
        double powerCommanded = 0;
        double scaledWheelRate = m_DesiredWheelSpeed / volts; // [rad/s/volt]
/*
        if ( scaledWheelRate >= -7.6 && scaledWheelRate < -1.2 )
        {
                powerCommanded = 0.3085*pow( scaledWheelRate, 3 ) + 4.3691*pow( scaledWheelRate, 2 ) - 94.8364*scaledWheelRate - 15.6830;
        }
        else if ( scaledWheelRate >= -1.2 && scaledWheelRate < 0 )
        {
                powerCommanded = 25.9997*pow( scaledWheelRate, 2 ) - 31.1477*scaledWheelRate + 25.968;
        }
        else if ( scaledWheelRate >= 0 && scaledWheelRate<= 1.2 )
        {
                powerCommanded = - 26.174*pow( scaledWheelRate, 2 ) - 31.1465*scaledWheelRate - 25.9364;
        }
        else if ( scaledWheelRate > 1.2 && scaledWheelRate <= 7.6 )
        {
                powerCommanded = 0.1015*pow( scaledWheelRate, 3 ) - 1.9227*pow( scaledWheelRate, 2 ) - 103.3392*scaledWheelRate + 24.3888;
        }
        else
        {
                // The Motor will maintain the previous voltage
                cerr << "\nWARNING: OUTSIDE POWER COMMANDED to MOTOR BOUNDS. No Power setting sent to Wheel Motor.  Wheel: " << m_WheelName 
                << "   Scaled Wheel Rate = " << scaledWheelRate << endl << endl;
                return; 
        }
*/

        if ( scaledWheelRate >= -7.6 && scaledWheelRate < -1.2 )
        {
                powerCommanded = (m_Coefficients1)(1)*pow( scaledWheelRate, 3 ) + (m_Coefficients1)(2)*pow( scaledWheelRate, 2 ) + (m_Coefficients1)(3)*scaledWheelRate + (m_Coefficients1)(4);
        }
        else if ( scaledWheelRate >= -1.2 && scaledWheelRate < 0 )
        {
                powerCommanded = (m_Coefficients2)(1)*pow( scaledWheelRate, 2 ) + (m_Coefficients2)(2)*scaledWheelRate + (m_Coefficients2)(3);
        }
        else if ( scaledWheelRate >= 0 && scaledWheelRate<= 1.2 )
        {
                powerCommanded = (m_Coefficients3)(1)*pow( scaledWheelRate, 2 ) + (m_Coefficients3)(2)*scaledWheelRate + (m_Coefficients3)(3);
        }
        else if ( scaledWheelRate > 1.2 && scaledWheelRate <= 7.6 )
        {
                powerCommanded = (m_Coefficients4)(1)*pow( scaledWheelRate, 3 ) +  (m_Coefficients4)(2)*pow( scaledWheelRate, 2 ) +  (m_Coefficients4)(3)*scaledWheelRate +  (m_Coefficients4)(4);
        }
        else
        {
                // The Motor will maintain the previous voltage
                cerr << "\nWARNING: OUTSIDE POWER COMMANDED to MOTOR BOUNDS. No Power setting sent to Wheel Motor.  Wheel: " << m_WheelName 
                << "   Scaled Wheel Rate = " << scaledWheelRate << endl << endl;
                return; 
        }

        TorqueCommand( powerCommanded );
        LAST_VOLT = volts;

        return;
}

void PhysicalMomentumWheel::StartPID( )
{
        pthread_create( &m_PIDThreadID, NULL, RunPIDController, (void*) this);
}

void* RunPIDController(void* arg)
{
        PhysicalMomentumWheel *wheel = (PhysicalMomentumWheel*) arg;
        double J = 0.04; //
        double c = 555.56;//unit conversion factor = 1 machine torque unit/0.0018 N-m
        double pi = 3.1415926535897;
        double timestep = 1000.0;
        double velocity;
        double desiredV;
        double error;
        double Kp;
        double Ki;
        double Kd;
        double torque;
        double TorqueChange;
        //static char *torqueCommand = new char[80];
        usleep(15000);
        while (1)
        {
                pthread_testcancel();

                cout << "PID THREAD RUNNING" << endl;

                pthread_mutex_lock(&wheel->m_Mutex);
                velocity = wheel->m_CurrentWheelSpeed/2.0/pi;
                desiredV = wheel->m_DesiredWheelSpeed/2.0/pi;//desired velocity in RPS
                pthread_mutex_unlock(&wheel->m_Mutex);
        
                error = desiredV - velocity;//current error in RPS
        
                cout << "current speed = " << velocity << endl;
                cout << "desired speed = " << desiredV << endl;
                cout << "error = " << error << endl;

                // Define gains
                Kp = 9.65*J/c;
        
                Ki = 0.0000055;
                if (fabs(error) >= 25 | fabs(desiredV) >= 25)
                {
                        Ki = 0.0000013;
                }
                
                Kd = 0.96*(fabs(error)*0.009029 + 1.92)*J/c; //linear regression equation
                if (fabs(error) >= 25 | fabs(desiredV) >= 25)
                { 
                        Kd = 2.14*J/c;
                }
        
        
                TorqueChange = c*(Kp*error + Kd*(error - wheel->LAST_ERR)/timestep + Ki*error*timestep);
        
                cout << "Torque Change = " << TorqueChange << endl;     

                torque = wheel->LAST_TOR + TorqueChange;

                cout << "torqueA: " << torque << endl;
                
                //Limit Excessive Torques changes
                if (abs((int) wheel->LAST_TOR - torque) > wheel->m_MaxTorqueStep  && torque != 0)
                {
                        if ((int) wheel->LAST_TOR >= 0 && (int) wheel->LAST_TOR < torque){
                                torque = (int) wheel->LAST_TOR + 300;}
                        else if ((int) wheel->LAST_TOR >= 0 && (int) wheel->LAST_TOR > torque){
                                torque = (int) wheel->LAST_TOR - 300;}
                        else if ((int) wheel->LAST_TOR < 0 && (int) wheel->LAST_TOR < torque){
                                torque = (int) wheel->LAST_TOR + 300;}
                        else if ((int) wheel->LAST_TOR < 0 && (int) wheel->LAST_TOR > torque){
                                torque = (int) wheel->LAST_TOR - 300;}
                }
                
                cout << "torque1: " << torque << endl;

                cout << "max: " << wheel->m_MaxWheelTorque << endl;

                // account for dead band
                if (torque >= 0 && torque < 25.5 )
                {
                        torque = torque + 25.5;
                }
                else if (torque < 0 && torque > -25.5 )
                {
                        torque = torque - 25.5;
                }
                else
                { }

                // Cap maximum torque values
                //if (abs(torque) > wheel->m_MaxWheelTorque)
                if (abs(torque) > 800 )
                {
                        torque = (int) wheel->LAST_TOR;
                }

                cout << "torque: " << torque << endl;

                wheel->TorqueCommand( -int(torque) );   
                /*
                sprintf(torqueCommand, "T%d\n\n", (int)torque);
                say(wheel->m_fd, torqueCommand);
                */

                pthread_testcancel();

                usleep(850000);
                /*      This loop allows for the motor to spin up with the current torque before another
                *       torque is applied. The allowable magnitude of spin up error is variable.  When the
                *       current wheel velocity is far away from the desired velocity, the error is large allowing
                *       for a faster spin up.  When the current velocity is close to the desired, the maximum error
                *       is small to all for more precise control. A break from the loop is provided for when
                *       the velocity error is very small, to keep from looping for an excessive amount of time.*/
                /*double spinuperror = wheel->LAST_VEL - velocity;
                while (fabs(spinuperror) > (fabs(error)/15)) {
                        wheel->LAST_VEL = velocity;
                        usleep(20000);
                        pthread_mutex_lock(&wheel->m_Mutex);
                        velocity = wheel->m_CurrentWheelSpeed/2.0/pi;
                        pthread_mutex_unlock(&wheel->m_Mutex);
                        spinuperror = wheel->LAST_VEL - velocity;
                        error = desiredV - velocity;
                        if (fabs(error)<0.05){ break;}
                        pthread_testcancel();
                }
                */
                wheel->LAST_VEL = velocity;
                wheel->LAST_ERR = error;
                wheel->LAST_TOR = torque;
                pthread_testcancel();
        }
        return arg;
}

void PhysicalMomentumWheel::StopPID()
{
        pthread_cancel(m_PIDThreadID);
        pthread_mutex_unlock(&m_Mutex);
}

void PhysicalMomentumWheel::CommandSystemTorque( double wheelTorque )
{
        //Old-Skool command a Pork in Tower mode function.
        pthread_mutex_lock(&m_Mutex);
        //Torque input a double in n-m  
        wheelTorque = -wheelTorque/0.0018;
        wheelTorque = round(wheelTorque);
        int torque;
        torque = (int) wheelTorque;
        //Limit Excessive Torques changes
        if (abs((int)LAST_TOR - torque) > m_MaxTorqueStep  && torque != 0)
        {
                if ((int) LAST_TOR >= 0 && (int) LAST_TOR < torque){
                        torque = (int) LAST_TOR + 600;}
                else if ((int) LAST_TOR >= 0 && (int) LAST_TOR > torque){
                        torque = (int) LAST_TOR - 600;}
                else if ((int) LAST_TOR < 0 && (int) LAST_TOR < torque){
                        torque = (int) LAST_TOR + 600;}
                else if ((int) LAST_TOR < 0 && (int) LAST_TOR > torque){
                        torque = (int) LAST_TOR - 600;}
        }

        // Cap maximum torque values
        if (abs(torque) > m_MaxWheelTorque)
        {
                torque = (int) LAST_TOR;
        }

        static char *torqueCommand = new char[80];
        sprintf(torqueCommand, "T%d\n\n", torque);
        say(m_fd, torqueCommand);

        m_CurrentWheelTorque = wheelTorque;
        
        //LAST_TOR = power ;

        pthread_mutex_unlock(&m_Mutex);
}



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

void PhysicalMomentumWheel::SetWheelTorqueLimits(const double& minWheelTorque, const double& maxWheelTorque)
{
    m_MaxWheelTorque = maxWheelTorque;
    m_MinWheelTorque = minWheelTorque;
}


void PhysicalMomentumWheel::SetMaxTorqueStep(double maxTorqueStep)
{
    m_MaxTorqueStep = maxTorqueStep;
}

void PhysicalMomentumWheel::SetWheelAddress(int WheelAddress)
{
        m_WheelAddress = 0x80 + WheelAddress;
}

void PhysicalMomentumWheel::SetDaisyChainNumber(int DaisyChainNumber)
{
        m_DaisyChainNumber = DaisyChainNumber;

}

void PhysicalMomentumWheel::SetDaisyChainPort(string DaisyChainPort)
{
        m_DaisyChainPort = DaisyChainPort;
}

void PhysicalMomentumWheel::SetBiasSpeed(double BiasSpeed)
{
        m_BiasSpeed = BiasSpeed;
        if( m_BiasSpeed != 0 )
        {
                m_BiasFlag = 1;
        }
}

void PhysicalMomentumWheel::SetAxialInertia(double _inertia)
{
        m_AxialInertia = _inertia;
}

void PhysicalMomentumWheel::SetWheelName( string _wheelName )
{
        m_WheelName = _wheelName;
}

void PhysicalMomentumWheel::SetWheelSpeedCoefficients( Vector _Coefficients1, Vector _Coefficients2, Vector _Coefficients3, Vector _Coefficients4 )
{
        (m_Coefficients1)(_(1,4)) = _Coefficients1;
        (m_Coefficients2)(_(1,3)) = _Coefficients2;
        (m_Coefficients3)(_(1,3)) = _Coefficients3;
        (m_Coefficients4)(_(1,4)) = _Coefficients4;
}
//////////////////////////////////////////////////////
// Inspectors
//////////////////////////////////////////////////////

void PhysicalMomentumWheel::GetWheelSpeed(double& Speed, double& measTime)
{
        pthread_mutex_lock(&m_Mutex);
        Speed = m_CurrentWheelSpeed;
        measTime = m_VelocityTime;
        pthread_mutex_unlock(&m_Mutex);
}
    

double PhysicalMomentumWheel::QueryWheelTorque()
{
        pthread_mutex_lock(&m_Mutex);
        //Clear Buffer
        static char *clear = new char[80];
        hear(m_fd, clear, 79, 0, 3333, '\r');
        
        //Send command  
        static char *RTCommand = new char[80];
        sprintf(RTCommand, "RT\n\n");
        say(m_fd, RTCommand);

        static char *torqueString = new char[80];
        hear(m_fd, torqueString, 20, 0, 10000, '\r');
        
        double Wtorque = atoi(torqueString+3) * 0.0018;//0.018 N-m per machine torque unit
        
        m_CurrentWheelTorque = Wtorque;
        
        delete clear;
        delete RTCommand;
        
        pthread_mutex_unlock(&m_Mutex);
        return Wtorque;
}

void PhysicalMomentumWheel::GetAxialInertia( double& _inertia )
{
        _inertia = m_AxialInertia;
}

double PhysicalMomentumWheel::GetAxialInertia( )
{
        return( m_AxialInertia );
}

string PhysicalMomentumWheel::GetWheelName( )
{
        return( m_WheelName );
}

void PhysicalMomentumWheel::GetWheelTorqueLimits(double& minWheelTorque, double& maxWheelTorque)
{
    maxWheelTorque = m_MaxWheelTorque;
    minWheelTorque = m_MinWheelTorque;
    return;
}


void PhysicalMomentumWheel::GetMaxTorqueStep(double& maxTorqueStep)
{
    maxTorqueStep = m_MaxTorqueStep;
    return;
}

int PhysicalMomentumWheel::GetWheelFD()
{
        return m_fd;
}

double PhysicalMomentumWheel::GetAmps()
{
        char *ampString = new char[80];
        double mamps;
        double amps;
        //clear serial port buffer
        //char* clear = new char[80];
        
        char *VarCommand = new char[80];
        sprintf(VarCommand, "a=UIA\rRa\n");
        
        pthread_mutex_lock(&m_Mutex);

                //while( !hear(m_fd, clear, 79, 0, 3333, 'c'));

                say(m_fd, VarCommand);
        
                //usleep(15000);
                //hear(m_fd, clear, 25, 0, 10000, '\n');

                //hear(m_fd, ampString, 15, 0, 10000, '\r');
        
                hear(m_fd, ampString, 15, 0, 60000, '\r');
        
        pthread_mutex_unlock(&m_Mutex);

        mamps = atoi(ampString);
        amps = mamps/100;
        
        return amps;
}

double PhysicalMomentumWheel::GetVolts()
{
        char *voltString = new char[80];
        double mvolts;
        double volts;
        //clear serial port buffer
        char* clear = new char[80];
        
        char *VarCommand = new char[80];
        sprintf(VarCommand, "b=UJA\rRb\n");
        
        pthread_mutex_lock(&m_Mutex);

                while( !hear(m_fd, clear, 79, 0, 3333, 'c'));

                say(m_fd, VarCommand);

                //usleep(15000);
                //hear(m_fd, clear, 25, 0, 10000, '\n');
        
                //hear(m_fd, voltString, 15, 0, 10000, '\r');
                hear(m_fd, voltString, 15, 0, 60000, '\r');
        pthread_mutex_unlock(&m_Mutex);
        
        mvolts = atoi(voltString);
        volts = mvolts/10;
        
        return volts;
}
// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: PhysicalMomentumWheel.cpp,v $
*       Revision 1.44  2007/08/31 15:48:03  jayhawk_hokie
*       Updated.
*       
*       Revision 1.43  2007/07/24 09:39:39  jayhawk_hokie
*       Removed matrix and vector pointers.
*       
*       Revision 1.42  2007/05/30 23:48:43  jayhawk_hokie
*       Modified thread termination.
*       
*       Revision 1.41  2007/05/23 22:09:16  jayhawk_hokie
*       Removed reducing wheel speed comments.
*       
*       Revision 1.40  2007/05/22 18:21:50  jayhawk_hokie
*       *** empty log message ***
*       
*       Revision 1.39  2007/04/09 14:01:31  jayhawk_hokie
*       Added capability in Torque Controller to reduce wheel speed.
*       
*       Revision 1.38  2007/03/27 19:39:02  jayhawk_hokie
*       Removed Unused Variable.
*       
*       Revision 1.37  2007/03/27 19:07:16  jayhawk_hokie
*       Updated Momentum Wheels.
*       
*       Revision 1.36  2007/02/06 20:56:23  jayhawk_hokie
*       Added SetAxialInertia function.
*       
*       Revision 1.35  2005/08/29 20:15:32  tjdpw
*       Removed unnecessary thread cancellations that caused seg fault.
*       
*       Revision 1.34  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.33  2004/05/11 20:18:08  bstreetman
*       Vast improvements to torque controller
*       
*       Revision 1.32  2004/04/19 15:00:43  bstreetman
*       Fixed a config and a shutdown bug
*       
*       Revision 1.31  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.
******************************************************************************/

---