PhysicalLinearActuator.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file PhysicalLinearActuator.cpp
*  \brief Implementation of the PhysicalLinearActuator class
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.3 $
*  \date    $Date: 2007/08/02 22:35:09 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////



#include "PhysicalLinearActuator.h"


    //////////////////////////////////////////////////
    // Contructors/Deconstructors                   //
    //////////////////////////////////////////////////

PhysicalLinearActuator::PhysicalLinearActuator()
{

        la.posvel = la.jogvel = 5;
        la.jogacc=la.posacc=la.posdec=20;       
        la.mtt=5000000;                         
        la.portfd= 0;
}

PhysicalLinearActuator::~PhysicalLinearActuator()
{

        //Deinitialize();

}

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

int PhysicalLinearActuator::TravelDistance(double param_distance)
{

        char *buffer;
        buffer = (char *) malloc(256);
        int InchesToSteps=25400;
        int DistanceSteps=static_cast<int>(floor(param_distance
                                *static_cast<double>(InchesToSteps)));
        say(la.portfd, "MR11");                         //portfd must be assigned       
        if (DistanceSteps < 0)
        
        {       sprintf(buffer, "DI%d\rFL\rSS-", -DistanceSteps); 
                say(la.portfd, buffer);
        }
        
        else
        
        {       sprintf(buffer, "DI%d\rFL\rSS+", DistanceSteps); 
                say(la.portfd, buffer);
        }
        return (0);
}

int PhysicalLinearActuator::TravelPosition(double param_position)
{
        char buffer[256];
        const int InchesToSteps=25400;

        int PositionSteps= static_cast<int>(floor(param_position
                                *static_cast<double>(InchesToSteps)));
        int LowerLimitSteps=static_cast<int>(floor(LowerLimit
                                *static_cast<double>(InchesToSteps)));
        int TravelSteps=static_cast<int>(floor(Travel
                                *static_cast<double>(InchesToSteps)));
        say(la.portfd, "MR11");
        sprintf(buffer, "DI%d\rFP", LowerLimitSteps+(TravelSteps/2)+PositionSteps); 
        say(la.portfd, buffer);
        
return (0);

}

int PhysicalLinearActuator::SetVelocity(double velocity)
{
        if (velocity < .025)
                velocity = .025;        
        char buffer[256];
        say(la.portfd, "MR11\r");
        sprintf(buffer, "CS%f\r", velocity);
        //sprintf(buffer, "CS%d\r", velocity);
        say(la.portfd, buffer);
        
        return (0);
}

int PhysicalLinearActuator::StartJog(int direction)
{
        if (direction != 0)
        {
                char buffer[256];       
                sprintf(buffer, "DI%d\r", direction);
                say(la.portfd, buffer);         
                say(la.portfd, "CJ\r");
                hear(la.portfd,buffer, 80, 0, 300000,1);
                return(0);
        }
        else  // use 0 for jog enable
        {               
                say(la.portfd, "JE\r");
                say(la.portfd, "JS1\r"); // set jog speed
                return(0);
        }       
}

int PhysicalLinearActuator::Stop()
{
        say(la.portfd, "ST\r");
        return (0);
}

/* This is pretty simply stuff.  We're going to open the port, play around
 * with the actuator as commanded, failing out if the error is critical
 * (like no comm, or hard faults), and return.  We DO NOT presume that
 * we're only called once, and we do not presume that the actuator is
 * is in an unknown state.  That's good and bad.  If the person who uses
 * this code doesn't know the actuator's state, a call to this will not
 * force them to actually do any initialization, as the bare minimum
 * that laInit() will do is open the port.  So as a minimum, if you're
 * actually initializing the LA for the first time in a program,
 * a minimum action flag of LAIT_COMM|LAIT_PRGP|LAIT_CTRA is recommended.
 * This will confirm a working link, program the presets, calculate the
 * real travel, and move the LA to a known position (LAIT_SLIM steps off the
 * farthest hard-limit).  If you don't do this, your program won't have
 * any way of knowing where the actuator is until it hits a hard-limit,
 * at which point you'll have to reset your counters and rescale, which
 * is exactly what the LAIT_PRGP and LAIT_CTRA flags are for in the
 * first place.  Of course, if you DO know where it is, you CAN set your
 * own counter and scale manually, and just use laInit to open the port
 * and do a comm check and not have to move the thing and possibly upset
 * your already stable system.  That's why we're not mandating travel checks.
 */
int PhysicalLinearActuator::Initialize(int action, char *port) 
{
  char *buffer,         /* R/W Buffer */
        *ptr;           /* A utlity pointer */
  int dir;              /* Direction of calc travel jog */
  int limflag=0;        /* Limit switch flag: 0 neither, 1 lo, 2 hi */
 
  la.portfn = port;
  buffer = (char *)malloc(256);
  init_serial(la.portfn, B9600, &(la.portfd));
  hear(la.portfd, buffer, 80, 0, 300000, 1);    /* Clear port buffer */

  // references to debug variable commented out since the compiler is unhappy
  //if (debug>4) printf("laInit(): action=%d\n", action);
  if (action&LAIT_COMM) {       /* Test controller comm */
    //if (debug) printf("laInit(): * * * LAIT_COMM * * *\n");
    say(la.portfd, "RS");       /* Get a status request */
    //if (debug>3) {
    //  printf("Set RS->\n");
    //  fflush(stdout);
    //}
    buffer[0] = 0;
    hear(la.portfd, buffer, 10, 0, 300000, '\r');
    //if (debug>3) {
    //  printf("Heard %s\n", buffer);
    //  fflush(stdout);
    //}
  
    switch(buffer[0]) {
      case 'M':                 /* Command in progress */
      case 'W':
      case 'T':
      close(la.portfd);
      return 1;

      case 'E':                 /* Servo error */
      close(la.portfd);
      return 2;

      case 'R':                 /* OK */
      break;

      default:                  /* Unknown or no response */
      close(la.portfd);
      return 3;
    }
  }

  if (action&LAIT_PRGP) {       /* Program presets into controller */
    //if (debug) printf("laInit(): * * * LAIT_PRGP * * *\n");
    sprintf(buffer,"VE%d", la.posvel);
    say(la.portfd, buffer);
    sprintf(buffer,"AC%d\r\nDE%d", la.posacc, la.posdec);
    say(la.portfd, buffer);
    sprintf(buffer,"JS%d\r\nJA%d", la.jogvel, la.jogacc);
    say(la.portfd, buffer);
  }


        say(la.portfd, "IS");
        hear(la.portfd, buffer, 30, 12, 300000, '\r');
        int old_position=atoi(buffer);






  if (action&LAIT_JPOS) {       /* Jog to positive end */
    //if (debug) printf("laInit(): * * * LAIT_JPOS * * *\n");
    say(la.portfd, "IS");
    hear(la.portfd, buffer, 30, 12, 300000, '\r');
    //if (debug>3) {
    //  printf("laInit(): ");
    //  printf("Got: %s\n", buffer);
    //}
    ptr = strstr(buffer, "IS=");
    //if (debug>3) printf("laInit(): Found: %s\n", ptr?ptr:"NULL");
    if (ptr?ptr[4] != '0':1) {  /* Reply mangled or not at limit */
      say(la.portfd, "DI1");
      say(la.portfd, "CJ");
      say(la.portfd, "SS+");
      hear(la.portfd, buffer, 5, 1, la.mtt, '\r');
    }
  }

  if (action&LAIT_JNEG) {       /* Jog to negative end */
    //if (debug) printf("laInit(): * * * LAIT_JNEG * * *\n");
    say(la.portfd, "IS");
    hear(la.portfd, buffer, 30, 12, 300000, '\r');
    ptr = strstr(buffer, "IS=");
    if (ptr?ptr[3] != '0':1) {  /* Reply mangled or not at limit */
      say(la.portfd, "DI-1");
      say(la.portfd, "CJ");
      say(la.portfd, "SS-");
      hear(la.portfd, buffer, 30, 1, la.mtt, '\r');
    }
  }

  /* This block is ugly, kludgy, hard to follow, but efficient.  The
   * latter is probably not a high priority since this is init code and
   * should not be performance critical.  A good programmer would rewrite
   * this to be longer, but easier to read.  Not me.  My apologies to anyone
   * trying to read this.  I've commented exhaustively to ease confusion.
   */
  if (action&LAIT_CTRA) {       /* Calc total travel, the fun stuff */
    //if (debug) printf("laInit(): * * * LAIT_CTRA * * *\n");
    say(la.portfd, "IS");                       /* Check limit switches */
    hear(la.portfd, buffer, 30, 12, 300000, '\r');
    ptr = strstr(buffer, "IS=");
    if (!ptr) {                         /* Mangled response */
      close(la.portfd);
      return(4);
    }
    if (ptr[3]=='0') limflag = 1;
    if (ptr[4]=='0') limflag = 2;

    /* First, we're going to check our current position (meaning check to
     * to see if either limit switch is tripped) and if so, do just one
     * full travel in the opposite direction.  If neither switch is tripped
     * we move to the closest one, if we know where we are.  If not, it
     * will just be random, so it won't matter that curpos, lo_l, and hi_l
     * are all uninitialized when we check them.
     */
    if (!limflag) {                     /* We're not at either limit */
      if ((la.curpos - la.lo_l) <       /* Current dist from low limit */
          (la.hi_l - la.curpos)) {      /* Current dist from hi limit */
        say(la.portfd, "DI-1"); /* We're nearer low limit, so... */
        limflag = 1;                    /* Manually set the limit flag */
      } else {                          /* We're nearer hi limit, so... */
        say(la.portfd, "DI1");          /* Move that way. */
        limflag = 2;                    /* Manually set the limit flag */
      }
      say(la.portfd, "CJ");             
      say(la.portfd, "SS-");
      hear(la.portfd, buffer, 30, 0, la.mtt, '\r');
    }
    /* By now, we're definitely AT ONE limit- so we'll do the measurement
     * travel based on which limit is tripped.
     */
    say(la.portfd, "SP");               /* Get pos- this is one lim pos */
    hear(la.portfd, buffer, 30, 12, 300000, '\r');  /* Record the response */
    /* So now, we're going to figure out which limit we're at, copy the
     * current position into that limit's marker, and set the direction
     * for the measurement travel accordingly.
     */
    if (limflag==2) {           /* We're at the positive limit */
      dir = 0;                          /* Set travel direction (0 is neg) */
      if (!(ptr = strstr(buffer, "SP="))) {
        close(la.portfd);
        return(4);
      }
      la.hi_l = atoi(ptr+3);            /* Record the high limit mark */
    } else {
      if (limflag==1) {         /* We're at the negative limit */
        dir = 1;                        /* Set travel direction (1 is pos) */
        if (!(ptr = strstr(buffer, "SP="))) {
          close(la.portfd);
          return(4);
        }
        la.lo_l = atoi(ptr+3);          /* Record the low limit mark */
      } else {
        close(la.portfd);               /* We shouldn't ever get here. */
        return(5);
      }
    }

    /* If originally at pos limit, dir=0, so go -, else dir=1, so go + ("") */
    sprintf(buffer, "DI%s1", ((char *)dir)?"":"-");
    say(la.portfd, buffer);     /* Now, let's jog away from the limit */
    say(la.portfd, "CJ");
    say(la.portfd, "SS.");
    hear(la.portfd, buffer, 10, 0, la.mtt, '\r');
    if (!buffer[0]) {           /* la.mtt passed but we're not at limit. */
      say(la.portfd, "SJ");     /* Kill the motor.  Safety demans that we not */
      close(la.portfd); /* press the issue here.  la.mtt is the user */
      return(5);                /* supplied limit-to-limit travel time.  It has */
    }                           /* passed, and we haven't tripped a switch.  Either */
                                /* the motor is dead, or the switch is broken and */
                                /* we've hit a stop.  Either case demands user */
                                /* inspection of the actuator. */
    /* Ok, so now we're at the opposite limit.  Let's take a reading, record
     * it in the structure, calculate the travel, and move off the limit.
     */
    say(la.portfd, "SP");               /* Get position */
    hear(la.portfd, buffer, 30, 12, 300000, '\r');  /* Record the response */
    if (!(ptr = strstr(buffer, "SP="))) {       /* Mangled response */
      close(la.portfd);
      return(4);
    }
    if (dir) la.hi_l = atoi(ptr+3);     /* If dir, hi limit is second */
      else la.lo_l = atoi(ptr+3);
                                        /* Now we calc travel and move. */
    la.travel = la.hi_l - la.lo_l;
    sprintf(buffer, "DI%s%d", ((char*)dir)?"-":"", LAIT_SLIM);
                                        /* Move LAIT_SLIM steps off limit */
    say(la.portfd, buffer);
    if (dir) la.curpos = (la.hi_l - la.lo_l) - LAIT_SLIM;
        else la.curpos = LAIT_SLIM;
  }

  if (action&LAIT_MOLD) {       /* Move to beginning position */
    //if (debug) printf("laInit(): * * * LAIT_MOLD * * *\n");
    sprintf(buffer, "DI%d\rFP", old_position); 
    say(la.portfd, buffer);
    hear(la.portfd, buffer, 10, 2, la.mtt, '\r');
    la.curpos = old_position;
  }

  if (action&LAIT_MPOS) {       /* Move to positive end */
    //if (debug) printf("laInit(): * * * LAIT_MPOS * * * (%d&%d)\n", action,
        //LAIT_MPOS);
    sprintf(buffer, "DI%d\rFP\rSS+", la.hi_l - LAIT_SLIM); 
    say(la.portfd, buffer);
    hear(la.portfd, buffer, 10, 2, la.mtt, '\r');
    la.curpos = la.travel-LAIT_SLIM;
  }

  if (action&LAIT_MNEG) {       /* Move to negative end */
    //if (debug) printf("laInit(): * * * LAIT_MNEG * * *\n");
    sprintf(buffer, "DI%d\rFP\rSS-", la.lo_l + LAIT_SLIM); 
    say(la.portfd, buffer);
    hear(la.portfd, buffer, 10, 2, la.mtt, '\r');
    la.curpos = LAIT_SLIM;
  }





UpperLimit=(static_cast <double>(la.hi_l))/25400.0;     //25400 is the number of steps in one inch, MR11
LowerLimit=(static_cast <double>(la.lo_l))/25400.0;
CurrentPosition=(static_cast <double>(la.curpos))/25400.0;
Travel=(static_cast <double>(la.travel))/25400.0;
 
return(0);

}


int PhysicalLinearActuator::Deinitialize() {
        //cout << "Serial Port Closing:\n";
        close(portfd);
        return 0;
}

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

double PhysicalLinearActuator::QueryCurrentPosition()
{
        char *buffer;
        buffer = (char *) malloc(256);
        say(la.portfd, "SP");                           /* Get position */
        hear(la.portfd, buffer, 30, 12, 300000, '\r');  /* Record the response */
        int currentposition=atoi(buffer);
        CurrentPosition=currentposition/25400.0;
        return CurrentPosition;
}
// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: PhysicalLinearActuator.cpp,v $
*       Revision 1.3  2007/08/02 22:35:09  jayhawk_hokie
*       Fixed Warning.
*
*       Revision 1.2  2006/06/27 15:27:12  bwilliam
*       Added jog functions. Added port file name and removed references to debug variable in initialization function.
*
*
*
*
*
****************************************************************************/
    

---