Initial commit of SINQ EPICS Application

2014-11-03 11:37:50 +01:00
commit fd4b41d131
36 changed files with 15416 additions and 0 deletions
--- a/sinqEPICSApp/src/EL734Driver.cpp
+++ b/sinqEPICSApp/src/EL734Driver.cpp
@@ -0,0 +1,430 @@
+/*
+FILENAME... EL734Driver.cpp
+USAGE...    Motor driver support for the PSI EL734 controller.
+
+Mark Koennecke
+February 2013
+
+*/
+
+
+#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)
+  :  asynMotorController(portName, numAxes+1, 0, 
+                         0, // No additional interfaces beyond those in base class
+                         0, // No additional callback interfaces beyond those in base class
+                         ASYN_CANBLOCK | ASYN_MULTIDEVICE, 
+                         1, // autoconnect
+                         0, 0)  // Default priority and stack size
+{
+  int axis;
+  asynStatus status;
+  EL734Axis *pAxis;
+  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++) {
+    pAxis = 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];
+  int status;
+  size_t in, out;
+  int reason;
+
+  strcpy(command,"RMT 1");
+  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
+				       reply,COMLEN, 1.,&out,&in,&reason); 
+  strcpy(command,"ECHO 0");
+  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
+				       reply,COMLEN, 1.,&out,&in,&reason); 
+  strcpy(command,"RMT 1");
+  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
+				       reply,COMLEN, 1.,&out,&in,&reason); 
+  strcpy(command,"ECHO 0");
+  status = 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(char command[COMLEN], char reply[COMLEN])
+{
+  asynStatus status;
+  size_t in, out, i;
+  int reason;
+  char myReply[COMLEN];
+
+  pasynOctetSyncIO->flush(pasynUserController_);
+  
+  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
+				       reply,COMLEN, 1.,&out,&in,&reason); 
+  if(status != asynSuccess){
+    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){
+    errlogSevPrintf(errlogMajor, "Bad command %s", command);
+    return asynError;
+  } else if(strstr(myReply,"?par") != NULL){
+    errlogSevPrintf(errlogMajor, "Bad parameter in command %s", command);
+    return asynError;
+  } else if(strstr(myReply,"?rng") != NULL){
+    errlogSevPrintf(errlogMajor, "Parameter out of range in command %s", command);
+    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)
+  : asynMotorAxis(pC, axisNo),
+    pC_(pC)
+{  
+}
+
+
+
+/** 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);
+  
+  if (relative) {
+    position += this->position;
+  }
+  oredMSR = 0;
+  homing = 0;
+  sprintf(command, "p %d %.3f", axisNo_, position/1000.);
+  status = pC_->transactController(command,reply);
+  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);
+
+  sprintf(command, "R %d", axisNo_);
+  homing = 1;
+  next_poll= -1;
+  status = pC_->transactController(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);
+    
+
+  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(command,reply);
+  next_poll = -1;
+  return status;
+}
+
+asynStatus EL734Axis::stop(double acceleration )
+{
+  asynStatus status;
+  //static const char *functionName = "EL734Axis::stop";
+  char command[COMLEN], reply[COMLEN];
+
+  sprintf(command, "S %d", axisNo_);
+  status = pC_->transactController(command,reply);
+  errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
+
+  return status;
+}
+
+asynStatus EL734Axis::setPosition(double position)
+{
+  asynStatus status;
+  //static const char *functionName = "EL734Axis::setPosition";
+  char command[COMLEN], reply[COMLEN];
+
+  sprintf(command, "P %d %f", axisNo_, position/1000.);
+  status = pC_->transactController(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;
+  asynStatus comStatus;
+  char command[COMLEN], reply[COMLEN];
+
+
+  // protect against excessive polling
+  if(time(NULL) < next_poll){
+    *moving = false;
+    return asynSuccess;
+  }
+
+  // Read the current motor position
+  sprintf(command,"u %d", axisNo_);
+  comStatus = pC_->transactController(command,reply);
+  if(comStatus) goto skip;
+  if(strstr(reply,"*ES") != NULL){
+    *moving = false;
+    setIntegerParam(pC_->motorStatusDone_, true);
+    setIntegerParam(pC_->motorStatusProblem_, true);
+    goto skip;
+  } else if(strstr(reply,"?BSY") != NULL){
+    *moving = true;
+    setIntegerParam(pC_->motorStatusDone_, false);
+    goto skip;
+  }
+  sscanf(reply,"%lf", &position);
+  //errlogPrintf("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(command,reply);
+  if(comStatus) goto skip;
+  sscanf(reply,"%x",&msr);
+  // errlogPrintf("Axis %d, reply %s, msr %d, position = %lf\n", 
+  // 	       axisNo_, reply, msr, position);
+  oredMSR |= msr;
+  if( (msr & 0x1) == 0){
+    // done: check for trouble
+    //errlogPrintf("Axis %d finished\n", axisNo_);
+    next_poll = time(NULL)+IDLEPOLL;
+    if(oredMSR & 0x10){
+      setIntegerParam(pC_->motorStatusLowLimit_, true);
+    }  else {
+      setIntegerParam(pC_->motorStatusLowLimit_, false);
+    }
+    if(oredMSR & 0x20){
+      setIntegerParam(pC_->motorStatusHighLimit_, true);
+    }  else {
+      setIntegerParam(pC_->motorStatusHighLimit_, false);
+    }
+    if(homing){
+      setIntegerParam(pC_->motorStatusAtHome_, true);
+    }
+    setIntegerParam(pC_->motorStatusProblem_, false);
+    if(oredMSR &0x1000){
+      setIntegerParam(pC_->motorStatusProblem_, true);
+      errlogSevPrintf(errlogMajor, "Air cushion problem on %d", axisNo_);
+    }
+    if(oredMSR &0x80){
+      setIntegerParam(pC_->motorStatusProblem_, true);
+      errlogSevPrintf(errlogMajor, "Positioning fault at %d", axisNo_);
+    }
+    *moving = false;
+    setIntegerParam(pC_->motorStatusDone_, true);
+  } else {
+    *moving = true;
+    next_poll = -1;
+    setIntegerParam(pC_->motorStatusDone_, false);
+  }
+
+  skip:
+  setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
+  callParamCallbacks();
+  return comStatus ? asynError : asynSuccess;
+}
+
+/** 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);
+}