MWCMGController.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file DefaultController.cpp
*  \brief Implementation of the DefaultController class.
*  \author $Author: cakinli $
*  \version $Revision: 1.6 $
*  \date    $Date: 2005/02/25 18:40:53 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

// This file implements the velocity feedback controller written by Eugene Skelton

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

int MWCMGController::Initialize() {
        
        LoadLUT("ReferenceMotion.txt");
        /** Set the contorl gain matrix */
        Matrix k(3,3);
        k(1,1) = 1;
        k(2,2) = 1;
        k(3,3) = 2;
        
        m_gainMatrix = k;
        A = FindA();
        MOI_sw = FindMOI_sw();
        gettimeofday(&InitialTime,NULL);
        
        return 0;
}

ofstream MRPoFile("MRPcurrent.txt",ios::out);




int MWCMGController::Run()
{

        /** Grabs the most recent parameters from the whorl object */
        Vector recentOmega_BL = m_whorl->GetOmegaBL();
        Matrix recentMOI = m_whorl->GetMOI();
        Vector Omega_s(3);
        double samplingTime;
        m_whorl->GetMomentumWheel("Wheel1")->GetWheelSpeed( Omega_s(1), samplingTime );
        m_whorl->GetMomentumWheel("Wheel2")->GetWheelSpeed( Omega_s(2), samplingTime );
        m_whorl->GetMomentumWheel("Wheel3")->GetWheelSpeed( Omega_s(3), samplingTime );
//      cout<<"get Omega_s"<<Omega_s<<endl;
        cout<<"Omega_BL = "<<recentOmega_BL<<endl;

        Vector Qcurrent(4);
        Qcurrent = m_whorl->GetQuaternion();
        //cout<<"Q = "<<Qcurrent<<endl;
        
        //I need to convert the Current quaternion to MRPs
        Vector MRPcurrent(3);
        MRPcurrent(1) = Qcurrent(1)/(1+Qcurrent(4)); 
        MRPcurrent(2) = Qcurrent(2)/(1+Qcurrent(4)); 
        MRPcurrent(3) = Qcurrent(3)/(1+Qcurrent(4)); 

        double MRPnorm;
        MRPnorm = MRPcurrent(1)*MRPcurrent(1)+MRPcurrent(2)*MRPcurrent(2)+MRPcurrent(3)*MRPcurrent(3);
                
        //cout<< "MRP^2 = "<<(MRPcurrent(1)*MRPcurrent(1)+MRPcurrent(2)*MRPcurrent(2)+MRPcurrent(3)*MRPcurrent(3)) << endl;
        if (MRPnorm > 1)
        {
                MRPcurrent(1) = -MRPcurrent(1)/MRPnorm;
                MRPcurrent(2) = -MRPcurrent(2)/MRPnorm;
                MRPcurrent(3) = -MRPcurrent(3)/MRPnorm; 
        //cout<< "MRP^2 = "<<(MRPcurrent(1)*MRPcurrent(1)+MRPcurrent(2)*MRPcurrent(2)+MRPcurrent(3)*MRPcurrent(3)) << endl;
        }
                
        //cout<<"MRPcurrent = "<<MRPcurrent<<endl;
        
        /**This is going to set the initial time if not already done. Then use difference to figure the current time*/
        gettimeofday(&CurrentTime,NULL);        //this line finds the current time in the maneuver
        
        double RunTime = static_cast<double>(CurrentTime.tv_sec - InitialTime.tv_sec) + (static_cast<double>(CurrentTime.tv_usec - InitialTime.tv_usec)) * 1e-6;
        cout<<"RunTime = "<<RunTime<<endl;

        /**Grabs the reference motion data for the most recent time*/
        double *myRow = tlu(RunTime);   //this line finds the vectors sigma,omega,and hdotref from the reference table.
        
//      cout<<"get myRow"<<myRow[0]<<"  "<<myRow[1]<<"  "<<myRow[2]<<"  "<<myRow[3]<<"  "<<myRow[4]<<"  "<<myRow[5]<<"  "<<myRow[6]<<"  "<<myRow[7]<<"  "<<myRow[8]<<"  "<<myRow[9]<<endl;
//      Vector MRPrefendl;
        Vector MRPRef(3);
        MRPRef(1) = myRow[3];
        MRPRef(2) = myRow[4];
        MRPRef(3) = myRow[5];
 // cout << "MRPRef = "<<MRPRef<<endl;

        Vector omegaRef(3);
        omegaRef(1) = myRow[0];
        omegaRef(2) = myRow[1];
        omegaRef(3) = myRow[2];
  //cout << "omegaRef"<<omegaRef<<endl;
        
        Vector hdotRef(3);
        hdotRef(1) = myRow[6];
        hdotRef(2) = myRow[7];
        hdotRef(3) = myRow[8];
  //cout << "hdotRef = "<<hdotRef<<endl;
        
        //cout<<"set stuff from myRow"<<MRPRef<<omegaRef<<hdotRef<<endl;
        /**Find the difference between the desired maneuver and the current*/
        Matrix RBR(3,3);  //RBR is the rotation between the body and reference frames
        Vector dMRP(3);
        Vector domega(3);
        
        dMRP = ((1-MRPcurrent(1)*MRPcurrent(1)-MRPcurrent(2)*MRPcurrent(2)-MRPcurrent(3)*MRPcurrent(3))*MRPRef +(1-MRPRef(1)*MRPRef(1)-MRPRef(2)*MRPRef(2)-MRPRef(3)*MRPRef(3))*MRPcurrent + 2*skew(MRPcurrent)*MRPRef)/(1+(MRPcurrent(1)*MRPcurrent(1)+MRPcurrent(2)*MRPcurrent(2)+MRPcurrent(3)*MRPcurrent(3))*(MRPRef(1)*MRPRef(1)+MRPRef(2)*MRPRef(2)+MRPRef(3)*MRPRef(3))+2*MRPcurrent(1)*MRPRef(1)+MRPcurrent(2)*MRPRef(2)+MRPcurrent(3)*MRPRef(3));

cout<<"dMRP " << dMRP << endl;

/**     We don't need this but I left it in to remind myself that it is not needed.
        if ((dMRP(1)*dMRP(1)+dMRP(2)*dMRP(2)+dMRP(3)*dMRP(3))>1)
        {
                dMRP(1) = -dMRP(1)/(dMRP(1)*dMRP(1)+dMRP(2)*dMRP(2)+dMRP(3)*dMRP(3));
                dMRP(2) = -dMRP(2)/(dMRP(1)*dMRP(1)+dMRP(2)*dMRP(2)+dMRP(3)*dMRP(3));
                dMRP(3) = -dMRP(3)/(dMRP(1)*dMRP(1)+dMRP(2)*dMRP(2)+dMRP(3)*dMRP(3));   
        cout<<"dMRP = "<< dMRP <<endl;
        }
*/              

        //Make a rotation matrix from the dMRP
        SIGMA = 1 - (dMRP(1)*dMRP(1) + dMRP(2)*dMRP(2) + dMRP(3)*dMRP(3)); 
        
        // Column 1
        RBR(1,1) = 4*(dMRP(1)*dMRP(1) - dMRP(2)*dMRP(2) - dMRP(3)*dMRP(3)) + SIGMA*SIGMA;
        RBR(2,1) = 8*dMRP(2)*dMRP(1) - 4*dMRP(3)*SIGMA;
        RBR(3,1) = 8*dMRP(3)*dMRP(1) + 4*dMRP(2)*SIGMA;
        // Column 2
        RBR(1,2) = 8*dMRP(1)*dMRP(2) + 4*dMRP(3)*SIGMA;
        RBR(2,2) = 4*(-dMRP(1)*dMRP(1) + dMRP(2)*dMRP(2) - dMRP(3)*dMRP(3)) + SIGMA*SIGMA;
        RBR(3,2) = 8*dMRP(3)*dMRP(2) - 4*dMRP(1)*SIGMA;
        // Column 3
        RBR(1,3) = 8*dMRP(1)*dMRP(3) - 4*dMRP(2) * SIGMA;
        RBR(2,3) = 8*dMRP(2)*dMRP(3) + 4*dMRP(1)*SIGMA;
        RBR(3,3) = 4*(-dMRP(1)*dMRP(1) - dMRP(2)*dMRP(2) + dMRP(3)*dMRP(3)) + SIGMA*SIGMA;

        RBR=1/pow((1+(dMRP(1)*dMRP(1) + dMRP(2)*dMRP(2) + dMRP(3)*dMRP(3))),2) * RBR;

        //cout<<"RBR = "<< RBR << endl;
        
        domega = recentOmega_BL + RBR * omegaRef;
        cout<<"domega"<<domega<<endl;
        
        //Make some controller gain matricces
        Matrix k1(3,3);  //dMRP Gains
        k1(1,1) = 1;
        k1(2,2) = 1;
        k1(3,3) = 2;
        
        Matrix k2(3,3); //domega Gains
        k2(1,1) = 1;
        k2(2,2) = 1;
        k2(3,3) = 1;
        


        
        /** Calculate the desired control torque */
//      m_controlTorque =-skew(recentOmega_BL)*(recentMOI*recentOmega_BL + MOI_sw*Omega_s) - recentMOI*skew(recentOmega_BL)*domega - RBR*(hdotRef) - m_gainMatrix*domega - m_gainMatrix*dMRP;
        m_controlTorque = - k1*dMRP - k2*domega ;
        cout<<"Control Torque = " << m_controlTorque<<endl;

        /** Set the desird contorl torque in the whorl object */
        m_whorl->SetControl(m_controlTorque);

        //hacking the controller to reject q drift
//      m_controlTorque(1)=0;
//      m_controlTorque(2)=0;
//      m_controlTorque(3)=0;   

        /** Set the torque for the momentum wheels to produce */
        if ( norm2(m_controlTorque) < 0.5 )
                SetWheelTorque(m_controlTorque); 
        else {
        m_controlTorque = 0.5 * ( m_controlTorque / norm2(m_controlTorque) );
        SetWheelTorque(m_controlTorque);
        };

        //      MRPoFile<<" " <<MRPcurrent<<" ";

        return 0;
}


// BUG: Will give crap data if it stops directly on the last line of data.

double *MWCMGController::tlu(double curtime) {
  int i;
  double *row;
  const double TOL = 0.001;

  // The table has ascending times (column 0) with a negative time to
  // mark the end of the table.

  //cout << "tlu(): Debug 1\n"; cout.flush();
  // Find the time that is less than what we've been given.
  for (i = 0;
        (lut[i][0] >= 0.0) && (lut[i][0] <= curtime) && fabs(lut[i][0] - curtime) > TOL;
        i++);

  //cout << "tlu(): Debug 2\n"; cout.flush();
  // If time on the row where we stopped is negative, we're dead,
  // because that means we ran of the end of the table.
  if (lut[i][0] < 0.0)
    return NULL;

  // Allocate the returned array (which must be delete'd later
  row = new double[9];

  // If we're dead on (defined as the diff being less than TOL), we just copy
  //cout << "tlu(): Debug 3\n"; cout.flush();
  //printf("Table entry (%d): %.5f   Ref Time: %.5f\n", i, lut[i][0], curtime);
  if (fabs(lut[i][0] - curtime) < TOL) {
    for (int j = 1; j <= 9; j++)
      row[j-1] = lut[i][j];
  // Otherwise, we do interpolation...
  } else {
    for (int j = 1; j <= 9; j++)
      row[j-1] = (curtime-lut[i-1][0])/(lut[i][0]-lut[i-1][0]) *
            (lut[i][j] - lut[i-1][j]) + lut[i-1][j];
  }
        
//cout<<"tlu(RunTime) makes myRow"<<row[1]<<endl;
//cout << "tlu(): Debug 4\n"; cout.flush();
  // Finally, return the row we've built.
  return row;
}


int MWCMGController::LoadLUT(char *file) {
  FILE *fp;
  char *line, *ptr;
  int lc = 0;
  const char *DELIM = "\t";
  int i,j;
  line = new char[1024];

  if (!(fp = fopen(file, "r")))
    return -1;

//  cout << "Debug 1\n";
//  cout.flush();
  // Count the number of lines in the data file
  while (fgets(line, 1023, fp)) lc++;
//  cout << "Debug 1.5\n";

  // Allocate lc rows and 10 columns
  lut = (double **)malloc(sizeof(double *)*(lc+1));
  for (int i = 0; i <= lc; i++)
    lut[i] = (double *)malloc(sizeof(double)*10);

//  cout << "Debug 2\n";
  // Rewind the file, and...
  rewind(fp);

  // Do the magic...  There's no error-checking for the right number
  // of columns.  If there aren't, we'll crap out.
  for (i = 0; fgets(line, 1023, fp); i++) {
    for (j = 0, ptr = strtok(line, DELIM);
                    j < 10;
                    j++, ptr = strtok(NULL, DELIM)) {
      if (ptr)
        lut[i][j] = atof(ptr);
      else {
        printf("Error loading table.\n");
        lut[i][j] = 0.0;
      }
    }
//  cout << "Debug loop: " << i << ' ' << j << endl;
  }
//  cout << "Debug 3\n";
  lut[i][0] = -1.0;
  fclose(fp);
  delete [] line;
  return 0;
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: MWCMGController.cpp,v $
*       Revision 1.6  2005/02/25 18:40:53  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.5  2003/10/24 20:56:11  cskelton
*       Modified for new GetWheelSpeed syntax.
*       
*       Revision 1.4  2003/08/21 23:32:33  cskelton
*       It works. Lots of couts are included to aid in controller debugging. ReferenceMotion.txt has to be in the directory of the .exe with columns of time, omega,MRP,hdot. The ability to save data that the controller loop is calc-ing hs been added (see MRPoFile)
*       
*       Revision 1.3  2003/08/21 13:58:35  cskelton
*       Controller updated. MRP shadow set added. TLU and LUT corrected.
*       
*       Revision 1.2  2003/08/15 16:12:07  cskelton
*       *** empty log message ***
*       
*       Revision 1.1  2003/08/15 04:50:14  cskelton
*       *** empty log message ***
*       
*       Revision 1.3  2003/08/14 18:10:41  mavandyk
*       Added momentum wheel speed querying.
*       
*       Revision 1.2  2003/08/13 23:17:20  mavandyk
*       Altered structure as outline in meeting.
*       
*       Revision 1.1  2003/07/31 20:43:14  mavandyk
*       This is the default controller for Whorl.
*       
*       Revision 1.3  2003/07/07 14:11:24  simpliciter
*       Removed GetWhorlObject() calls following Whorl pointers.
*       
*       
*
******************************************************************************/

---