sinqepicsapp/sinqEPICSApp/src/EL734Driver.cpp

/*
FILENAME... EL734Driver.cpp
USAGE...    Motor driver support for the PSI EL734 controller.

Mark Koennecke
February 2013

Updated to have an MsgTxt field through SINQAxis, error handling

Mark Koennecke, May, August 2017

*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <time.h>

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

#include <asynOctetSyncIO.h>

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

#define IDLEPOLL 60

/** Creates a new EL734Controller object.
 * \param[in] portName          The name of the asyn port that will be created for this driver
 * \param[in] EL734PortName     The name of the drvAsynSerialPort that was created previously to connect to the EL734 controller
 * \param[in] numAxes           The number of axes that this controller supports
 */
EL734Controller::EL734Controller(const char *portName, const char *EL734PortName, int numAxes)
    : SINQController(portName, EL734PortName, numAxes)
{
  int axis;
  asynStatus status;
  static const char *functionName = "EL734Controller::EL734Controller";

  /* Connect to EL734 controller */
  status = pasynOctetSyncIO->connect(EL734PortName, 0, &pasynUserController_, NULL);
  if (status)
  {
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
              "%s: cannot connect to EL734 controller\n",
              functionName);
  }
  pasynOctetSyncIO->setOutputEos(pasynUserController_, "\r", strlen("\r"));
  pasynOctetSyncIO->setInputEos(pasynUserController_, "\r", strlen("\r"));

  switchRemote();

  for (axis = 0; axis < numAxes; axis++)
  {
    new EL734Axis(this, axis + 1);
  }

  startPoller(1000. / 1000., IDLEPOLL, 2);
}

/** Creates a new EL734Controller 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] EL734PortName       The name of the drvAsynIPPPort that was created previously to connect to the EL734 controller
 * \param[in] numAxes           The number of axes that this controller supports
 */
extern "C" int EL734CreateController(const char *portName, const char *EL734PortName, int numAxes)
{
  EL734Controller *pEL734Controller = new EL734Controller(portName, EL734PortName, numAxes);
  pEL734Controller = 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 it calls asynMotorController::report()
 */
void EL734Controller::report(FILE *fp, int level)
{
  fprintf(fp, "EL734 motor driver %s, numAxes=%d\n",
          this->portName, numAxes_);

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

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

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

void EL734Controller::switchRemote()
{
  char command[COMLEN], reply[COMLEN];
  size_t in, out;
  int reason;

  strcpy(command, "RMT 1");
  pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                              reply, COMLEN, 1., &out, &in, &reason);
  strcpy(command, "ECHO 0");
  pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                              reply, COMLEN, 1., &out, &in, &reason);
  strcpy(command, "RMT 1");
  pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                              reply, COMLEN, 1., &out, &in, &reason);
  strcpy(command, "ECHO 0");
  pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                              reply, COMLEN, 1., &out, &in, &reason);
}
/**
 * send a command to the EL734 and read the reply. Do some error and controller
 * issue fixing on the way
 * \param[in] command The command to send
 * \param[out] reply The controllers reply
 */

asynStatus EL734Controller::transactController(int axisNo, char command[COMLEN], char reply[COMLEN])
{
  asynStatus status;
  size_t in, out, i;
  int reason;
  char myReply[COMLEN], errTxt[256];
  SINQAxis *axis = getAxis(axisNo);

  pasynOctetSyncIO->flush(pasynUserController_);

  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                                       reply, COMLEN, 2., &out, &in, &reason);
  if (status != asynSuccess)
  {
    if (axis != NULL)
    {
      axis->updateMsgTxtFromDriver("Lost connection to motor controller");
    }
    return status;
  }

  /*
     check for the offline reply
  */
  if (strstr(reply, "?LOC") != NULL)
  {
    switchRemote();
    return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                                       reply, COMLEN, 1., &out, &in, &reason);
  }

  /*
    check for echos. This means that the thing is offline.
  */
  if (strstr(reply, "p ") != NULL || strstr(reply, "u ") != NULL || strstr(reply, "msr ") != NULL)
  {
    switchRemote();
    return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
                                       reply, COMLEN, 1., &out, &in, &reason);
  }
  /*
    check for EL734 errors
  */
  strcpy(myReply, reply);
  for (i = 0; i < strlen(reply); i++)
  {
    myReply[i] = (char)tolower((int)reply[i]);
  }
  if (strstr(myReply, "?cmd") != NULL)
  {
    snprintf(errTxt, sizeof(errTxt), "Bad command %s at axis %d", command, axisNo);
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt);
    if (axis != NULL)
    {
      axis->updateMsgTxtFromDriver(errTxt);
    }
    return asynError;
  }
  else if (strstr(myReply, "?par") != NULL)
  {
    snprintf(errTxt, sizeof(errTxt), "Bad parameter in command %s", command);
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt);
    if (axis != NULL)
    {
      axis->updateMsgTxtFromDriver(errTxt);
    }
    return asynError;
  }
  else if (strstr(myReply, "?rng") != NULL)
  {
    snprintf(errTxt, sizeof(errTxt), "Parameter out of range in command %s", command);
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt);
    if (axis != NULL)
    {
      axis->updateMsgTxtFromDriver(errTxt);
    }
    return asynError;
  }

  return status;
}

// These are the EL734Axis methods

/** Creates a new EL734Axis object.
 * \param[in] pC Pointer to the EL734Controller to which this axis belongs.
 * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
 *
 * Initializes register numbers, etc.
 */
EL734Axis::EL734Axis(EL734Controller *pC, int axisNo)
    : SINQAxis(pC, axisNo), pC_(pC)
{
  char command[COMLEN], reply[COMLEN];
  asynStatus status;
  int count = 0;
  float low, high;

  /*
    get the hardware limits from the controller
  */
  sprintf(command, "H %d", axisNo_);
  status = pC_->transactController(axisNo_, command, reply);
  if (status == asynSuccess)
  {
    count = sscanf(reply, "%f %f", &low, &high);
    if (count >= 2)
    {
      pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, low);
      pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, high);
      callParamCallbacks();
    }
    else
    {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
                "Bad response - %s - requesting limits at axis %d", reply, axisNo_);
    }
  }
  else
  {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
              "Failed to read limits at axis %d", axisNo_);
  }

  /*
  oredMSR contains the axis status from the previous poll. The initial value
  1 means that the axis is not moving and has no errors.
  */
  oredMSR = 1;
}

/** Reports on status of the axis
 * \param[in] fp The file pointer on which report information will be written
 * \param[in] level The level of report detail desired
 *
 * After printing device-specific information calls asynMotorAxis::report()
 */
void EL734Axis::report(FILE *fp, int level)
{
  if (level > 0)
  {
    fprintf(fp, "  axis %d\n",
            axisNo_);
  }

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

asynStatus EL734Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  // static const char *functionName = "EL734Axis::move";
  char command[COMLEN], reply[COMLEN];

  // status = sendAccelAndVelocity(acceleration, maxVelocity);

  errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity);

  /*
   * set speed
   */
  sprintf(command, "J %d %d", axisNo_, (int)maxVelocity);
  status = pC_->transactController(axisNo_, command, reply);

  if (relative)
  {
    position += this->position;
  }
  homing = 0;
  errorReported = 0;
  asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
            "Starting axis %d with destination %f", axisNo_, position / 1000);
  sprintf(command, "p %d %.3f", axisNo_, position / 1000.);
  status = pC_->transactController(axisNo_, command, reply);
  setIntegerParam(pC_->motorStatusProblem_, false);
  updateMsgTxtFromDriver("");
  next_poll = -1;
  return status;
}

asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
  asynStatus status;
  // static const char *functionName = "EL734Axis::home";
  char command[COMLEN], reply[COMLEN];

  // status = sendAccelAndVelocity(acceleration, maxVelocity);

  setIntegerParam(pC_->motorStatusProblem_, false);
  updateMsgTxtFromDriver("");
  errorReported = 0;

  sprintf(command, "R %d", axisNo_);
  homing = 1;
  next_poll = -1;
  status = pC_->transactController(axisNo_, command, reply);
  return status;
}

asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  // static const char *functionName = "EL734Axis::moveVelocity";
  char command[COMLEN], reply[COMLEN];

  // asynPrint(pasynUser_, ASYN_TRACE_FLOW,
  //   "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
  //   functionName, minVelocity, maxVelocity, acceleration);

  errorReported = 0;
  if (maxVelocity > 0.)
  {
    /* This is a positive move */
    sprintf(command, "FF %d", axisNo_);
  }
  else
  {
    /* This is a negative move  */
    sprintf(command, "FB %d", axisNo_);
  }
  status = pC_->transactController(axisNo_, command, reply);
  setIntegerParam(pC_->motorStatusProblem_, false);
  updateMsgTxtFromDriver("");
  next_poll = -1;
  return status;
}

asynStatus EL734Axis::stop(double acceleration)
{
  asynStatus status = asynSuccess;
  // static const char *functionName = "EL734Axis::stop";
  char command[COMLEN], reply[COMLEN];
  bool moving = false;

  this->poll(&moving);
  if (moving && errorReported == 0)
  {
    sprintf(command, "S %d", axisNo_);
    status = pC_->transactController(axisNo_, command, reply);
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Sent STOP on Axis %d\n", axisNo_);
    updateMsgTxtFromDriver("Axis interrupted");
    errorReported = 1;
  }

  return status;
}

asynStatus EL734Axis::setPosition(double position)
{
  asynStatus status;
  // static const char *functionName = "EL734Axis::setPosition";
  char command[COMLEN], reply[COMLEN];

  sprintf(command, "U %d %f", axisNo_, position / 1000.);
  status = pC_->transactController(axisNo_, command, reply);
  next_poll = -1;

  return status;
}

asynStatus EL734Axis::setClosedLoop(bool closedLoop)
{
  // static const char *functionName = "EL734Axis::setClosedLoop";

  /*
    This belongs into the Kingdom of Electronics.
    We do not do this.
   */

  return asynError;
}

/** Polls the axis.
 * This function reads the motor position, the limit status, the home status, the moving status,
 * and the drive power-on status.
 * It calls setIntegerParam() and setDoubleParam() for each item that it polls,
 * and then calls callParamCallbacks() at the end.
 * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus EL734Axis::poll(bool *moving)
{
  int msr, count;
  float low, high;
  asynStatus comStatus = asynSuccess;
  char command[COMLEN], reply[COMLEN], errTxt[256];

  // protect against excessive polling
  if (time(NULL) < next_poll)
  {
    *moving = false;
    return asynSuccess;
  }

  // read hardware limits
  sprintf(command, "H %d", axisNo_);
  comStatus = pC_->transactController(axisNo_, command, reply);
  if (comStatus == asynSuccess)
  {
    count = sscanf(reply, "%f %f", &low, &high);
    if (count >= 2)
    {
      pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, low);
      pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, high);
      callParamCallbacks();
    }
  }

  // Read the current motor position
  setIntegerParam(pC_->motorStatusProblem_, false);
  sprintf(command, "u %d", axisNo_);
  comStatus = pC_->transactController(axisNo_, command, reply);
  if (comStatus == asynError)
  {
    setIntegerParam(pC_->motorStatusProblem_, true);
    goto skip;
  }
  if (strstr(reply, "*ES") != NULL)
  {
    *moving = false;
    setIntegerParam(pC_->motorStatusDone_, true);
    setIntegerParam(pC_->motorStatusProblem_, true);
    errorReported = 1;
    updateMsgTxtFromDriver("Emergency Stop Engaged");
    comStatus = asynError;
    goto skip;
  }
  else if (strstr(reply, "?BSY") != NULL)
  {
    *moving = true;
    setIntegerParam(pC_->motorStatusDone_, false);
    goto skip;
  }
  count = sscanf(reply, "%lf", &position);
  if (count != 1)
  {
    if (!homing)
    {
      snprintf(errTxt, sizeof(errTxt), "Bad reply %s when reading position for %d", reply, axisNo_);
      setIntegerParam(pC_->motorStatusProblem_, true);
      errorReported = 1;
      updateMsgTxtFromDriver(errTxt);
      comStatus = asynError;
      goto skip;
    }
  }
  else
  {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
              "Axis %d, reply %s, position %lf\n", axisNo_, reply, position);
    setDoubleParam(pC_->motorPosition_, position * 1000);
    // setDoubleParam(pC_->motorEncoderPosition_, position);
  }

  // Read the moving status of this motor
  sprintf(command, "msr %d", axisNo_);
  comStatus = pC_->transactController(axisNo_, command, reply);
  if (comStatus == asynError)
  {
    setIntegerParam(pC_->motorStatusProblem_, true);
    goto skip;
  }
  sscanf(reply, "%x", &msr);

  // errlogPrintf("Axis %d, reply %s, msr %d, oredmsr = %d, position = %lf\n",
  //	       axisNo_, reply, msr, oredMSR, position);

  oredMSR |= msr;
  if ((msr & 0x1) == 0)
  {
    // done: check for trouble
    // errlogPrintf("Axis %d finished\n", axisNo_);
    *moving = false;
    setIntegerParam(pC_->motorStatusDone_, true);

    next_poll = time(NULL) + IDLEPOLL;
    if (oredMSR & 0x10)
    {
      setIntegerParam(pC_->motorStatusLowLimit_, true);
      updateMsgTxtFromDriver("Lower Limit Hit");
      errorReported = 1;
      comStatus = asynError;
      goto skip;
    }
    else
    {
      setIntegerParam(pC_->motorStatusLowLimit_, false);
    }
    if (oredMSR & 0x20)
    {
      setIntegerParam(pC_->motorStatusHighLimit_, true);
      updateMsgTxtFromDriver("Upper Limit Hit");
      errorReported = 1;
      comStatus = asynError;
      goto skip;
    }
    else
    {
      setIntegerParam(pC_->motorStatusHighLimit_, false);
    }
    if (homing)
    {
      setIntegerParam(pC_->motorStatusAtHome_, true);
    }
    if (oredMSR & 0x1000)
    {
      setIntegerParam(pC_->motorStatusProblem_, true);
      //      errlogPrintf("Detected air cushion error on %d", axisNo_);
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Air cushion problem on %d", axisNo_);
      updateMsgTxtFromDriver("Air cushion error");
      errorReported = 1;
      comStatus = asynError;
      goto skip;
    }
    if (oredMSR & 0x100)
    {
      setIntegerParam(pC_->motorStatusProblem_, true);
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Run failure at %d", axisNo_);
      updateMsgTxtFromDriver("Run failure");
      comStatus = asynError;
      errorReported = 1;
      goto skip;
    }
    if (oredMSR & 0x400)
    {
      setIntegerParam(pC_->motorStatusProblem_, true);
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Positioning failure at %d", axisNo_);
      updateMsgTxtFromDriver("Positioning failure");
      comStatus = asynError;
      errorReported = 1;
      goto skip;
    }
    if (oredMSR & 0x200 || oredMSR & 0x80)
    {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, "Positioning fault at %d", axisNo_);
    }
    setIntegerParam(pC_->motorStatusProblem_, false);
  }
  else
  {
    *moving = true;
    next_poll = -1;
    setIntegerParam(pC_->motorStatusDone_, false);
  }

skip:
  callParamCallbacks();
  return comStatus;
}

/** Code for iocsh registration */
static const iocshArg EL734CreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg EL734CreateControllerArg1 = {"EL734 port name", iocshArgString};
static const iocshArg EL734CreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg *const EL734CreateControllerArgs[] = {&EL734CreateControllerArg0,
                                                            &EL734CreateControllerArg1,
                                                            &EL734CreateControllerArg2};
static const iocshFuncDef EL734CreateControllerDef = {"EL734CreateController", 3, EL734CreateControllerArgs};
static void EL734CreateContollerCallFunc(const iocshArgBuf *args)
{
  EL734CreateController(args[0].sval, args[1].sval, args[2].ival);
}

static void EL734Register(void)
{
  iocshRegister(&EL734CreateControllerDef, EL734CreateContollerCallFunc);
}

extern "C"
{
  epicsExportRegistrar(EL734Register);
}