PhysicalControlMomentGyro.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file PhysicalControlMomentGyro.cpp
*  \brief Implementation of the PhysicalControlMomentGyro class
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.20 $
*  \date    $Date: 2007/08/02 22:31:05 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "PhysicalControlMomentGyro.h"
#include <fstream>

int PhysicalControlMomentGyro::m_fd = -1;
/////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


PhysicalControlMomentGyro::PhysicalControlMomentGyro()
{
}

PhysicalControlMomentGyro::~PhysicalControlMomentGyro()
{
        Stop();
}


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


void PhysicalControlMomentGyro::CommandWheelSpeed(double wheelOmega)
{
        wheelOmega = wheelOmega*64425.156/(2*3.14159265359); //64425.156 cts/s / rps * 1 rps/ 2pi rad/s

        wheelOmega = round(wheelOmega); // motor accepts only integer velocities

        static char *speedCommand = new char[80];
        sprintf(speedCommand, "%cV%d\n", m_WheelAddress, (int)wheelOmega);
        say(m_fd, speedCommand);

        GoCommand(m_WheelAddress);

}



void PhysicalControlMomentGyro::CommandWheelTorque(double wheelTorque)
{
        
        //Torque input a double in n-m  
        wheelTorque = wheelTorque/0.0018;
        wheelTorque = floor(wheelTorque);
        int torque;
        torque = (int) wheelTorque;
        static char *torqueCommand = new char[80];

        sprintf(torqueCommand, "%cMT\n\n", m_WheelAddress);
        say(m_fd, torqueCommand);
        
        sprintf(torqueCommand, "%cT%d\n\n", m_WheelAddress, torque);
        say(m_fd, torqueCommand);

        m_CurrentWheelTorque = wheelTorque;
}


void PhysicalControlMomentGyro::CommandAngle(double wheelAngle)
{
        // wheelAngle should be radians
        // Angles measured as a change from the mid-point of the cmg's rotation limits
        
        wheelAngle = wheelAngle/0.000010224; // 1.0224e-5 rad/ct
        wheelAngle = round(wheelAngle);
        
        int DesiredLAPosition;

        DesiredLAPosition = m_LAMid + (int)wheelAngle;

        if (DesiredLAPosition > m_LAMax){
                DesiredLAPosition = m_LAMax;
        } else if (DesiredLAPosition < m_LAMin){
                DesiredLAPosition = m_LAMin;
        }
        
        static char *AngleCommand = new char[80];
        sprintf(AngleCommand, "%cP%d\n\n", m_LAAddress, DesiredLAPosition);

        say(m_fd, AngleCommand);
        GoCommand(m_LAAddress);
        
        m_CurrentAngle = DesiredLAPosition*0.000010224;
}

void PhysicalControlMomentGyro::CommandGimbalRate(double gimbalRate)
{
        //gimbal rate in rad/sec
        gimbalRate = gimbalRate * 1.0/0.000010224 * 1.0/2000.0 * 32212.578; // 1 rad/s(in gimbal axis) * 1 pos ct/1.0224e-5 rad (linear axis)* 1 rev/2000 ct (la motor speed)* 32212.578 scaled ct/ 1 rev/s

        // The initial rad/s number times (1/2pi) rev/rad *32212.578 (counts/s)/(rev/s)
        //gimbalRate=gimbalRate*(1/2*3.14159265)*32212.578;
        gimbalRate = round(gimbalRate);
        
        
        static char *off = new char[80];
        sprintf(off, "%c%s\n\n", m_LAAddress, "OFF");
        say(m_fd, off);
        
        static char *velCommand = new char[80];
        sprintf(velCommand, "%cMV\n\n", m_LAAddress);
        say(m_fd, velCommand);

        static char *accCommand = new char[80];
        sprintf(accCommand, "%cA%d\n\n", m_LAAddress, 3);
        say(m_fd, accCommand);  

        static char *GimbalCommand = new char[80];
        sprintf(GimbalCommand, "%cV%f\n\n", m_LAAddress, gimbalRate);
        //sprintf(GimbalCommand, "%cV%d\n\n", m_LAAddress, gimbalRate);
        
        say(m_fd, GimbalCommand);
        GoCommand(m_LAAddress);
}


int PhysicalControlMomentGyro::Stop()
{
        CommandWheelSpeed(0.0);
        CommandWheelTorque(0.0);
        return 0;
}


int PhysicalControlMomentGyro::Initialize()
{

        m_WheelAddress = 130;
        m_LAAddress = 129;

        
        char *stringPort;
        stringPort = (char *)malloc(80);
        
        cout << m_DaisyChainPort << endl;
        if (m_fd < 0) { // g_fd has NOT been set yet, then init the daisychain
          sprintf(stringPort,"/dev/tty%s",m_DaisyChainPort.c_str());

          init_serial(stringPort,B9600,&m_fd);  

          cout << stringPort << endl;
        
          char *testCommand;
          testCommand = new char[80];
        
          sprintf(testCommand, "%c%s\x0D", 0x80, "Z");
          say(m_fd, testCommand); 
          usleep(500000);
        
          sprintf(testCommand, "%c%s\x0D", 0x80, "SADDR1");
          say(m_fd, testCommand); 
                
          usleep(500000);
          sprintf(testCommand, "%c%s\x0D", 0x81, "ECHO");
          say(m_fd, testCommand); 
          usleep(500000);

          sprintf(testCommand, "%c%s\x0D", 0x81, "SLEEP");
          say(m_fd, testCommand); 
          usleep(500000);

          sprintf(testCommand, "%c%s\x0D", 0x80, "SADDR2");
          say(m_fd, testCommand); 
          usleep(500000);

          sprintf(testCommand, "%c%s\x0D", 0x82, "ECHO");
          say(m_fd, testCommand); 
          usleep(500000);

          sprintf(testCommand, "%c%s\x0D", 0x81, "WAKE");
          say(m_fd, testCommand); 
          usleep(500000);
         
          
          FindLimits();

          sleep(3);

          //Set slower LA speed for actual travel
          sprintf(testCommand, "%c%s\x0D", 0x81, "V50000");
          say(m_fd, testCommand);
          usleep(500000);
          
          GoCommand(m_LAAddress);


          //Set up Wheel

          sprintf(testCommand, "%c%s\x0D", 0x82, "MV");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "KI0");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "KP50");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "KD50");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "KL200");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "F");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "V1830000");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x82, "A25");
          say(m_fd, testCommand); 
          usleep(500000);
          
          GoCommand(m_WheelAddress);
                
/*        sprintf(testCommand, "%c%s\x0D", 0x82, "V1700000");
          say(m_fd, testCommand); 
          usleep(500000);https://webmail.vt.edu/MBX/timj/ID=43229BB9
          
          GoCommand(m_WheelAddress);

          sleep(15);
*/        
          cout << "\nCMG Initialized" << endl;
          
        }
        return 0;
}


int PhysicalControlMomentGyro::Deinitialize()
{
        close(m_fd);
        return 0;
}



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


void PhysicalControlMomentGyro::SetWheelSpeedLimits(double minWheelSpeed, double maxWheelSpeed)
{
        m_MinWheelSpeed = minWheelSpeed;
        m_MaxWheelSpeed = maxWheelSpeed;
}


void PhysicalControlMomentGyro::SetWheelTorqueLimits(double minWheelTorque,double maxWheelTorque)
{
        m_MinWheelTorque = minWheelTorque;
        m_MaxWheelTorque = maxWheelTorque;
}


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

void PhysicalControlMomentGyro::SetDaisyChainNumber(int DaisyChainNumber)
{
        m_DaisyChainNumber = DaisyChainNumber;
}

void PhysicalControlMomentGyro::SetDaisyChainPort(string Port)
{
        m_DaisyChainPort = Port;
}
/////////////////////////////////////////////////////////////////////////////
// Inspectors
/////////////////////////////////////////////////////////////////////////////


double PhysicalControlMomentGyro::QueryWheelSpeed()
{
        //clear serial port buffer
        char* clear = new char[80];
        while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
        
        
        //begin commanding and reading
        int pos1 = 0;
        int pos2 = 0;
        struct timeval *time1 = new struct timeval;
        struct timeval *time2 = new struct timeval;
        int dt;
        char *posString = new char[80];
        double velocity;
        int readdelay =10000;
        
        
        char *RPCommand = new char[80];
        sprintf(RPCommand, "%cRP\n", m_WheelAddress);
        say(m_fd, RPCommand);
        
        usleep(10000);  
        
        hear(m_fd, posString, 79, 0, 1000000, '\n');

        hear(m_fd, posString, 79, 0, 1000000, '\r');

        pos1 = atoi(posString);
        
        gettimeofday(time1,NULL);
        
        usleep(readdelay);
        
        //position2
        say(m_fd, RPCommand);
        
        usleep(10000);  
        
        hear(m_fd, posString, 15, 0, 1000000, '\n');
        
        hear(m_fd, posString, 79, 0, 1000000, '\r');
        
        pos2 = atoi(posString);

        gettimeofday(time2,NULL);
        
        delete RPCommand;
        delete posString;
        
        
        dt = time2->tv_usec - time1->tv_usec;
        
        if (dt<0) { 
                dt = 1000000 + dt;
        }

        velocity = ((pos2 - pos1)/((double) dt))*2*3.14159*250.0;// rad/sec -> 2pi rad/4000 ct * 1000000 usec/sec
        
        
        m_CurrentWheelSpeed = velocity;
        return m_CurrentWheelSpeed;
}
    

double PhysicalControlMomentGyro::QueryWheelTorque()
{
        
        //clear serial port buffer
        char* clear = new char[80];
        while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
        
        //Send command  
        static char *RTCommand = new char[80];
        sprintf(RTCommand, "%cRT\n\n", m_WheelAddress);
        say(m_fd, RTCommand);
        
        static char *torqueString = new char[80];
        hear(m_fd, torqueString, 20, 0, 1000000, '\n');
        
        hear(m_fd, torqueString, 20, 0, 1000000, '\r');

        double Wtorque = atoi(torqueString) * 0.0018;//0.0018 N-m per machine torque unit
        
        m_CurrentWheelTorque = Wtorque;
        
        
        return m_CurrentWheelTorque;
}


double PhysicalControlMomentGyro::QueryAngle()
{
        //clear serial port buffer
        char* clear = new char[80];
        while( !hear(m_fd, clear, 79, 0, 10000, 'c'));

        //send command
        static char *RPCommand = new char[80];
        sprintf(RPCommand, "%cRP\n\n", m_LAAddress);
        say(m_fd, RPCommand);

        static char *posString = new char[80];
        hear(m_fd, posString, 15, 0, 1000000, '\n');

        hear(m_fd, posString, 79, 0, 1000000, '\r');

        int LAPosition = atoi(posString);

        m_CurrentAngle = (LAPosition - m_LAMid)*0.000010224;//1.0224e-5 rad/ct

        return m_CurrentAngle;
        
}


void PhysicalControlMomentGyro::GetWheelSpeedLimits(double& minWheelSpeed, double& maxWheelSpeed)
{
        minWheelSpeed = m_MinWheelSpeed;
        maxWheelSpeed = m_MaxWheelSpeed;
}


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


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


void PhysicalControlMomentGyro::GoCommand(int Address)
{
        // generic command G
        // input either wheel address or la address
        static char *GoString = new char[80];
        sprintf(GoString, "%cG\n", Address);
        say(m_fd, GoString);
}


void PhysicalControlMomentGyro::FindLimits()
{
          //Setup LA
          char *testCommand = new char[80];
        
          sprintf(testCommand, "%c%s\x0D", 0x81, "MP");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x81, "F1");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x81, "V500000");
          say(m_fd, testCommand); 
          usleep(500000);
          
          sprintf(testCommand, "%c%s\x0D", 0x81, "A1000");
          say(m_fd, testCommand);
          usleep(500000);

          //find limits
        //clear serial port buffer
        char* clear = new char[80];
        while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
        
        char* ErrorString = new char[80];
        sprintf(ErrorString, "%cRBp\n\n", m_LAAddress);
        say(m_fd, ErrorString);
        
        hear(m_fd, ErrorString, 79, 0, 1000000, '\n');
        
        hear(m_fd, ErrorString, 79, 0, 1000000, '\r');
        int PosLimit = atoi(ErrorString);
        
        while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
        
        sprintf(ErrorString, "%cRBm\n\n", m_LAAddress);
        say(m_fd, ErrorString);
        
        hear(m_fd, ErrorString, 79, 0, 1000000, '\n');
        hear(m_fd, ErrorString, 79, 0, 1000000, '\r');
        int NegLimit = atoi(ErrorString);

        if (PosLimit){
                sprintf(testCommand, "%cP1000000\n", m_LAAddress);
                say(m_fd, testCommand);
                GoCommand(m_LAAddress);
                sleep(5);
                while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
                sprintf(ErrorString, "%cRP\n\n", m_LAAddress);
                say(m_fd, ErrorString);
                usleep(10000);
                
                hear(m_fd, ErrorString, 79, 0, 1000000, '\n');
                
                hear(m_fd, ErrorString, 79, 0, 1000000, '\r');
                m_LAMax = atoi(ErrorString);
                m_LAMin = 0;
                
        } else if(NegLimit){
                sprintf(testCommand, "%cP-1000000\n", m_LAAddress);
                say(m_fd, testCommand);
                GoCommand(m_LAAddress);
                sleep(5);
                while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
                sprintf(ErrorString, "%cRP\n\n", m_LAAddress);
                say(m_fd, ErrorString);
                
                usleep(10000);
                hear(m_fd, ErrorString, 79, 0, 1000000, '\n');
                
                hear(m_fd, ErrorString, 79, 0, 1000000, '\r');
                m_LAMin = atoi(ErrorString);
                m_LAMax = 0;
        } else {
                sprintf(testCommand, "%cP1000000\n", m_LAAddress);
                say(m_fd, testCommand);
                GoCommand(m_LAAddress);
                sleep(5);
                while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
                sprintf(ErrorString, "%cRP\n\n", m_LAAddress);
                say(m_fd, ErrorString);
                usleep(10000);
                
                hear(m_fd, ErrorString, 79, 0, 1000000, '\n');
                hear(m_fd, ErrorString, 79, 0, 1000000, '\r');
                m_LAMax = atoi(ErrorString);
                sprintf(testCommand, "%cP-1000000\n", m_LAAddress);
                say(m_fd, testCommand);
                GoCommand(m_LAAddress);
                sleep(7);
                while( !hear(m_fd, clear, 79, 0, 10000, 'c'));
                sprintf(ErrorString, "%cRP\n\n", m_LAAddress);
                say(m_fd, ErrorString);
                usleep(10000);
                
                hear(m_fd, ErrorString, 79, 0, 1000000, '\n');
                hear(m_fd, ErrorString, 79, 0, 1000000, '\r');
                m_LAMin = atoi(ErrorString);
        }
        double LAMid = ( m_LAMax + m_LAMin)/2.0;
        m_LAMid = (int)LAMid;
        sprintf(testCommand, "%cP%d", m_LAAddress, m_LAMid);
        say(m_fd, testCommand);
        GoCommand(m_LAAddress);
}

---