Experimental Physics and
Industrial Control System

/*
FILENAME... PXMotorDriver.cpp
USAGE...    Motor driver support for the Parker PX series of controllers, including the Aries.

Mark Rivers
March 4, 2011

*/


#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include <iocsh.h>
#include <epicsThread.h>

#include <asynOctetSyncIO.h>

#include "PXMotorDriver.h"
#include <epicsExport.h>

#define CtlY 25
#define PX_TIMEOUT 1.0

static const char *driverName = "PXMotorDriver";

/** Creates a new PXController object.
  * \param[in] portName          The name of the asyn port that will be created for this driver
  * \param[in] PXPortName       The name of the drvAsynIPPPort that was created previously to connect to the PX controller 
  * \param[in] numAxes           The number of axes that this controller supports 
  * \param[in] movingPollPeriod  The time between polls when any axis is moving 
  * \param[in] idlePollPeriod    The time between polls when no axis is moving 
*/
PXController::PXController(const char *portName, const char *PXPortName, int numAxes, 
                             double movingPollPeriod, double idlePollPeriod)
  :  asynMotorController(portName, numAxes, NUM_PX_PARAMS, 
                         asynInt32Mask | asynFloat64Mask | asynUInt32DigitalMask, 
                         asynInt32Mask | asynFloat64Mask | asynUInt32DigitalMask,
                         ASYN_CANBLOCK | ASYN_MULTIDEVICE, 
                         1, // autoconnect
                         0, 0)  // Default priority and stack size
{
  int axis;
  asynStatus status;
  PXAxis *pAxis;
  static const char *functionName = "PXController";

  binaryInReg_  = 4096;
  binaryOutReg_ = 4097;
  
  // Create controller-specific parameters
  createParam(PXReadBinaryIOString, asynParamInt32,         &PXReadBinaryIO_);
  createParam(PXBinaryInString,     asynParamUInt32Digital, &PXBinaryIn_);
  createParam(PXBinaryOutString,    asynParamUInt32Digital, &PXBinaryOut_);
  createParam(PXBinaryOutRBVString, asynParamUInt32Digital, &PXBinaryOutRBV_);

  /* Connect to PX controller */
  status = pasynOctetSyncIO->connect(PXPortName, 0, &pasynUserPX_, NULL);
  if (status) {
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
      "%s:%s: cannot connect to PX controller\n",
      driverName, functionName);
  }
  // Turn off command echoing
  sprintf(outString_, "BIT1792=0");
  writeController();
   // Turn off command prompt
  sprintf(outString_, "BIT1794=1");
  writeController();
  // Wait a short while so that any responses to the above commands have time to arrive so we can flush
  // them in the next writeReadController()
  epicsThreadSleep(0.5);
  // Read the binary I/O registers once
  readBinaryIO();
  // Set the output=output readback so bi records reflect current state
  setUIntDigitalParam(0, PXBinaryOut_, binaryOutRBV_, 0xFFFFFFFF);
  // Create the axis objects
  for (axis=0; axis<numAxes; axis++) {
    pAxis = new PXAxis(this, axis);
  }

  startPoller(movingPollPeriod, idlePollPeriod, 2);
}


/** Creates a new PXController object.
  * Configuration command, called directly or from iocsh
  * \param[in] portName          The name of the asyn port that will be created for this driver
  * \param[in] PXPortName       The name of the drvAsynIPPPort that was created previously to connect to the PX controller 
  * \param[in] numAxes           The number of axes that this controller supports 
  * \param[in] movingPollPeriod  The time in ms between polls when any axis is moving
  * \param[in] idlePollPeriod    The time in ms between polls when no axis is moving 
  */
extern "C" int PXCreateController(const char *portName, const char *PXPortName, int numAxes, 
                                   int movingPollPeriod, int idlePollPeriod)
{
  PXController *pPXController
    = new PXController(portName, PXPortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
  pPXController = NULL;
  return(asynSuccess);
}

/** Reports on status of the driver
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * If details > 0 then information is printed about each axis.
  * After printing controller-specific information calls asynMotorController::report()
  */
void PXController::report(FILE *fp, int level)
{
  int axis;
  PXAxis *pAxis;

  fprintf(fp, "PX motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", 
    this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);

  if (level > 0) {
    fprintf(fp, "  binary input = 0x%x\n", binaryIn_);
    fprintf(fp, "  binary output readback = 0x%x\n", binaryOutRBV_);
    for (axis=0; axis<numAxes_; axis++) {
      pAxis = getAxis(axis);
      fprintf(fp, "  axis %d\n"
              "  pulsesPerUnit_ = %f\n"
              "    encoder position=%f\n"
              "    theory position=%f\n"
              "    limits=0x%x\n"
              "    flags=0x%x\n", 
              pAxis->axisNo_, pAxis->pulsesPerUnit_, 
              pAxis->encoderPosition_, pAxis->theoryPosition_,
              pAxis->currentLimits_, pAxis->currentFlags_);
    }
  }

  // Call the base class method
  asynMotorController::report(fp, level);
}

/** Returns a pointer to an PXMotorAxis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
PXAxis* PXController::getAxis(asynUser *pasynUser)
{
  return static_cast<PXAxis*>(asynMotorController::getAxis(pasynUser));
}

/** Returns a pointer to an PXMotorAxis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] axisNo Axis index number. */
PXAxis* PXController::getAxis(int axisNo)
{
  return static_cast<PXAxis*>(asynMotorController::getAxis(axisNo));
}



asynStatus PXController::writeUInt32Digital(asynUser *pasynUser, epicsUInt32 value, epicsUInt32 mask)
{
  int bit, tmask=0x1;
  asynStatus status;
  //static const char *functionName = "writeUInt32Digital";
  
  for (bit=0; bit<32; bit++) {
    if (mask & tmask) break;
    tmask = tmask << 1;
  }
  sprintf(outString_, "BIT %d=%d", 32+bit, value);
  status = writeController();
  // Read the I/O back
  readBinaryIO();

  return(status);
}

/** Reads the binary input and binary output registers on the PX.
  * Sets the values in the parameter library.
  * Keeps track of which bits have changed.
  * Calls any registered callbacks for this pasynUser->reason and address. */ 
asynStatus PXController::readBinaryIO()
{
  asynStatus status;

  // Read the binary inputs
  sprintf(outString_, "?P%d", binaryInReg_);
  status = writeReadController();
  if (!status) {
    binaryIn_ = atoi(inString_);
    setUIntDigitalParam(0, PXBinaryIn_, binaryIn_, 0xFFFFFFFF);
  }

  // Read the binary outputs
  sprintf(outString_, "?P%d", binaryOutReg_);
  status = writeReadController();
  if (!status) {
    binaryOutRBV_  = atoi(inString_);
    setUIntDigitalParam(0, PXBinaryOutRBV_, binaryOutRBV_, 0xFFFFFFFF);
  }
  callParamCallbacks(0);
  return status;
}

/** Writes a string to the PX controller.
  * Calls writeController() with a default location of the string to write and a default timeout. */ 
asynStatus PXController::writeController()
{
  return writeController(outString_, PX_TIMEOUT);
}

/** Writes a string to the PX controller.
  * \param[in] output The string to be written.
  * \param[in] timeout Timeout before returning an error.*/
asynStatus PXController::writeController(const char *output, double timeout)
{
  size_t nwrite;
  asynStatus status;
  // const char *functionName="writeController";
  
  status = pasynOctetSyncIO->write(pasynUserPX_, output,
                                   strlen(output), timeout, &nwrite);
                                  
  return status ;
}

/** Writes a string to the PX controller and reads the response.
  * Calls writeReadController() with default locations of the input and output strings
  * and default timeout. */ 
asynStatus PXController::writeReadController()
{
  size_t nread;
  return writeReadController(outString_, inString_, sizeof(inString_), &nread, PX_TIMEOUT);
}

/** Writes a string to the PX controller and reads a response.
  * \param[in] output Pointer to the output string.
  * \param[out] input Pointer to the input string location.
  * \param[in] maxChars Size of the input buffer.
  * \param[out] nread Number of characters read.
  * \param[out] timeout Timeout before returning an error.*/
asynStatus PXController::writeReadController(const char *output, char *input, size_t maxChars, size_t *nread, double timeout)
{
  size_t nwrite;
  asynStatus status;
  int eomReason;
  // const char *functionName="writeReadController";
  
  status = pasynOctetSyncIO->writeRead(pasynUserPX_, output,
                                       strlen(output), input, maxChars, timeout,
                                       &nwrite, nread, &eomReason);
                        
  return status;
}


// These are the PXAxis methods

/** Creates a new PXAxis object.
  * \param[in] pC Pointer to the PXController to which this axis belongs. 
  * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
  * 
  * Initializes register numbers, etc.
  */
PXAxis::PXAxis(PXController *pC, int axisNo)
  : asynMotorAxis(pC, axisNo),
    pC_(pC)
{
  asynStatus status;
  
  sprintf(axisName_, "AXIS%d", axisNo);
  encoderPositionReg_ = 12290 + 256*axisNo;
  theoryPositionReg_  = 12294 + 256*axisNo;
  limitsReg_          = 4600  + axisNo;
  flagsReg_           = 4120  + axisNo;
  // Get the number of pulses per unit on this axis
  sprintf(pC->outString_, "%s PPU", axisName_);
  status = pC->writeReadController();
  if (status) {
    setIntegerParam(pC->motorStatusProblem_, 1);
  } else {
    pulsesPerUnit_ = atof(pC->inString_);
    // We assume servo motor with encoder for now
    setIntegerParam(pC->motorStatusGainSupport_, 1);
    setIntegerParam(pC->motorStatusHasEncoder_, 1);
  }
  callParamCallbacks();
}

asynStatus PXAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  // static const char *functionName = "moveAxis";

  sprintf(pC_->outString_, "%s STX P1 %f %f CR", axisName_, maxVelocity/pulsesPerUnit_, position/pulsesPerUnit_);
  status = pC_->writeController();
  return status;
}

asynStatus PXAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
  asynStatus status;
  // static const char *functionName = "homeAxis";
sprintf(pC_->outString_, "%s STX H %f CR", axisName_, maxVelocity/pulsesPerUnit_);
  status = pC_->writeController();
  return status;
}

/** Code for iocsh registration */
static const iocshArg PXCreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg PXCreateControllerArg1 = {"PX port name", iocshArgString};
static const iocshArg PXCreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg PXCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
static const iocshArg PXCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
static const iocshArg * const PXCreateControllerArgs[] = {&PXCreateControllerArg0,
                                                           &PXCreateControllerArg1,
                                                           &PXCreateControllerArg2,
                                                           &PXCreateControllerArg3,
                                                           &PXCreateControllerArg4};
static const iocshFuncDef PXCreateControllerDef = {"PXCreateController", 5, PXCreateControllerArgs};
static void PXCreateContollerCallFunc(const iocshArgBuf *args)
{
  PXCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
}

static void PXMotorRegister(void)
{
  iocshRegister(&PXCreateControllerDef, PXCreateContollerCallFunc);
}

extern "C" {
epicsExportRegistrar(PXMotorRegister);
}

/*
FILENAME...   PXMotorDriver.h
USAGE...      Motor driver support for the Parker ACR series of controllers, including the Aries.

Mark Rivers
March 28, 2011

*/

#include "asynMotorController.h"
#include "asynMotorAxis.h"

/** drvInfo strings for extra parameters that the PX controller supports */
//#define PXJerkString           "PX_JERK"
#define PXReadBinaryIOString   "PX_READ_BINARY_IO"
#define PXBinaryInString       "PX_BINARY_IN"
#define PXBinaryOutString      "PX_BINARY_OUT"
#define PXBinaryOutRBVString   "PX_BINARY_OUT_RBV"

#define MAX_PX_STRING_SIZE 85

class PXAxis : public asynMotorAxis
{
public:
  /* These are the methods we override from the base class */
  PXAxis(class PXController *pC, int axis);
  asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
  /*asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);*/
  asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
 //asynStatus stop(double acceleration);
 // asynStatus poll(bool *moving);
  //asynStatus setPosition(double position);

private:
  PXController *pC_;      /**< Pointer to the asynMotorController to which this axis belongs.
                                *   Abbreviated because it is used very frequently */
  char axisName_[10];      /**< Name of each axis, used in commands to PX controller */ 
  double pulsesPerUnit_;   /**< Pulses per engineering unit, which is what PX controller uses */ 
  int flagsReg_;           /**< Address of the flags register */ 
  int limitsReg_;          /**< Address of the limits register */ 
  int encoderPositionReg_; /**< Address of the encoder position register */ 
  int theoryPositionReg_;  /**< Address of the theoretical position register */ 
  double encoderPosition_; /**< Cached copy of the encoder position */ 
  double theoryPosition_;  /**< Cached copy of the theoretical position */ 
  int currentFlags_;       /**< Cached copy of the current flags */ 
  int currentLimits_;      /**< Cached copy of the current limits */ 
  
friend class PXController;
};

class PXController : public asynMotorController {
public:
  PXController(const char *portName, const char *PXPortName, int numAxes, double movingPollPeriod, double idlePollPeriod);

  /* These are the methods that we override from asynPortDriver */
  asynStatus writeUInt32Digital(asynUser *pasynUser, epicsUInt32 value, epicsUInt32 mask);
  
  /* These are the methods that we override from asynMotorDriver */
 /* asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
  #asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);*/
  void report(FILE *fp, int level);
  PXAxis* getAxis(asynUser *pasynUser);
  PXAxis* getAxis(int axisNo);

  
  /* These are the methods that are new to this class */
  asynStatus readBinaryIO();
  asynStatus writeController();
  asynStatus writeController(const char *output, double timeout);
  asynStatus writeReadController();
  asynStatus writeReadController(const char *output, char *response, size_t maxResponseLen, size_t *responseLen, double timeout);
  
//protected:
  /*int PXJerk_;          **< Jerk time parameter index*/ 
    int PXReadBinaryIO_; 
#define FIRST_PX_PARAM PXReadBinaryIO_
  //int PXReadBinaryIO_;  **< Read binary I/O parameter index */
  int PXBinaryIn_;      /**< Binary input parameter index */
  int PXBinaryOut_;     /**< Binary output parameter index */
  int PXBinaryOutRBV_;  /**< Binary output readback parameter index */
#define LAST_PX_PARAM PXBinaryOutRBV_ 

#define NUM_PX_PARAMS  (&LAST_PX_PARAM - &FIRST_PX_PARAM + 1)

private:
  asynUser *pasynUserPX_;
  char outString_[MAX_PX_STRING_SIZE];
  char inString_[MAX_PX_STRING_SIZE];
  int binaryIn_;
  int binaryOutRBV_;
  int binaryInReg_;
  int binaryOutReg_;
  
friend class PXAxis;
};

Replies:

Re: How to support a new stepper motor controller? Mark Rivers

RE: How to support a new stepper motor controller? Mark Rivers

References:

How to support a new stepper motor controller? Ralph Lange

Re: How to support a new stepper motor controller? Kevin Peterson

Re: How to support a new stepper motor controller? Rod Nussbaumer

Navigate by Date:

Prev: Re: [epics-modules/asyn] Throw exception if we can't register interfaces (#43) Andrew Johnson

Next: Re: How to support a new stepper motor controller? Mark Rivers

Index: 1994  1995  1996  1997  1998  1999  2000  2001  2002  2003  2004  2005  2006  2007  2008  2009  2010  2011  2012  2013  2014  2015  2016  <2017>  2018  2019  2020  2021  2022  2023  2024 

Navigate by Thread:

Prev: Re: How to support a new stepper motor controller? Rod Nussbaumer

Next: Re: How to support a new stepper motor controller? Mark Rivers

Index: 1994  1995  1996  1997  1998  1999  2000  2001  2002  2003  2004  2005  2006  2007  2008  2009  2010  2011  2012  2013  2014  2015  2016  <2017>  2018  2019  2020  2021  2022  2023  2024