A first sort of working version of a asyn driver for the Nanotoec SMCI

motors. Homing still needs more testing
2015-07-30 14:12:45 +02:00
parent fd4b41d131
commit cdf309114a
10 changed files with 631 additions and 2 deletions
--- a/sinqEPICSApp/src/NanotecDriver.cpp
+++ b/sinqEPICSApp/src/NanotecDriver.cpp
@@ -0,0 +1,451 @@
+/*
+FILENAME... NanotecDriver.cpp
+USAGE...    Motor driver support for the Nanotec SMCI controllers 
+            connected to a RS-485 bus.
+
+Mark Koennecke
+July 2015
+
+*/
+
+
+#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 "stptok.h"
+#include "NanotecDriver.h"
+#include <epicsExport.h>
+
+#define IDLEPOLL 60
+
+
+/** Creates a new NanotecController object.
+  * \param[in] portName          The name of the asyn port that will be created for this driver
+  * \param[in] NanotecPortName     The name of the drvAsynSerialPort that was created previously to connect to the Nanotec controller 
+  */
+NanotecController::NanotecController(const char *portName, const char *NanotecPortName, int motCount, const char *bus)
+  :  asynMotorController(portName, motCount+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, busAddress;
+  asynStatus status;
+  NanotecAxis *pAxis;
+  static const char *functionName = "NanotecController::NanotecController";
+  char *pPtr, busNoString[20];
+
+
+  /* Connect to Nanotec controller */
+  status = pasynOctetSyncIO->connect(NanotecPortName, 0, &pasynUserController_, NULL);
+  if (status) {
+    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
+      "%s: cannot connect to Nanotec controller\n",
+      functionName);
+  }
+  pasynOctetSyncIO->setOutputEos(pasynUserController_,"\r",strlen("\r"));
+  pasynOctetSyncIO->setInputEos(pasynUserController_,"\r",strlen("\r"));
+
+  axis = 1;
+  pPtr = (char *)bus;
+  while((pPtr = stptok(pPtr,busNoString,sizeof(busNoString),",")) != NULL){
+    busAddress = atoi(busNoString);
+    pAxis = new NanotecAxis(this, axis,busAddress);
+    errlogPrintf("Axis %d, busAddress = %d\n", axis, busAddress);
+    axis++;
+  }
+
+  startPoller(1000./1000., IDLEPOLL, 2);
+}
+
+
+/** Creates a new NanotecController 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] NanotecPortName       The name of the drvAsynIPPPort that was created previously to connect to the Nanotec controller 
+  */
+extern "C" int NanotecCreateController(const char *portName, const char *NanotecPortName, int numMot, const char *busAddresses)
+{
+  NanotecController *pNanotecController
+    = new NanotecController(portName, NanotecPortName,numMot,busAddresses);
+  pNanotecController = 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 NanotecController::report(FILE *fp, int level)
+{
+  fprintf(fp, "Nanotec motor driver %s, numAxes=%d\n", 
+    this->portName, numAxes_);
+
+  // Call the base class method
+  asynMotorController::report(fp, level);
+}
+
+/** Returns a pointer to an NanotecAxis object.
+  * Returns NULL if the axis number encoded in pasynUser is invalid.
+  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
+NanotecAxis* NanotecController::getAxis(asynUser *pasynUser)
+{
+  return static_cast<NanotecAxis*>(asynMotorController::getAxis(pasynUser));
+}
+
+/** Returns a pointer to an NanotecAxis object.
+  * Returns NULL if the axis number encoded in pasynUser is invalid.
+  * \param[in] axisNo Axis index number. */
+NanotecAxis* NanotecController::getAxis(int axisNo)
+{
+  return static_cast<NanotecAxis*>(asynMotorController::getAxis(axisNo));
+}
+
+
+
+// These are the NanotecAxis methods
+
+/** Creates a new NanotecAxis object.
+  * \param[in] pC Pointer to the NanotecController to which this axis belongs. 
+  * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
+  * 
+  * Initializes register numbers, etc.
+  */
+NanotecAxis::NanotecAxis(NanotecController *pC, int axisNo, int busAddress)
+  : asynMotorAxis(pC, axisNo),
+    pC_(pC)
+{  
+  this->busAddress = busAddress;
+
+}
+
+
+
+/** 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 NanotecAxis::report(FILE *fp, int level)
+{
+  if (level > 0) {
+    fprintf(fp, "  axis %d\n",
+            axisNo_);
+  }
+
+  // Call the base class method
+  //asynMotorAxis::report(fp, level);
+}
+
+asynStatus NanotecController::transactController(char command[COMLEN], char reply[COMLEN])
+{
+  asynStatus status;
+  size_t in, out;
+  int reason;
+
+  pasynOctetSyncIO->flush(pasynUserController_);
+  
+  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
+				       reply,COMLEN, 1.,&out,&in,&reason); 
+  if(status != asynSuccess){
+    return status;
+  }
+
+  /*
+    check for Nanotec errors
+  */
+  if(strstr(reply,"?") != NULL){
+    errlogSevPrintf(errlogMajor, "Bad command %s", command);
+    return asynError;
+  }
+
+  if(strlen(reply) < 1) {
+    errlogSevPrintf(errlogMajor, "No reply received for  %s", command);
+    return asynError;
+  }
+
+  return status;
+}
+
+asynStatus NanotecAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
+{
+  asynStatus status;
+  //static const char *functionName = "NanotecAxis::move";
+  char command[COMLEN], reply[COMLEN];
+  size_t in, out;
+  int reason;
+
+  // status = sendAccelAndVelocity(acceleration, maxVelocity);
+  
+  if (relative) {
+    position += this->position;
+  }
+
+  homing = 0;
+
+  /*
+    set mode
+  */
+  snprintf(command,sizeof(command),"#%dp2",busAddress);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+  
+  /*
+    set target
+  */
+  snprintf(command,sizeof(command),"#%ds%d",busAddress, (int)position);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+  
+  /*
+    and start..
+  */
+  snprintf(command,sizeof(command),"#%dA",busAddress);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+
+  next_poll = -1;
+  return status;
+}
+
+asynStatus NanotecAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
+{
+  asynStatus status;
+  //static const char *functionName = "NanotecAxis::home";
+  char command[COMLEN], reply[COMLEN];
+
+  /*
+    set mode
+  */
+  snprintf(command,sizeof(command),"#%dp4",busAddress);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+  
+  /*
+    reset positioning errors
+  */
+  snprintf(command,sizeof(command),"#%dD",busAddress);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+
+  /*
+    set direction
+  */
+  snprintf(command,sizeof(command),"#%dd0",busAddress);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+  
+
+  /*
+    and start..
+  */
+  snprintf(command,sizeof(command),"#%dA",busAddress);
+  status = pC_->transactController(command,reply);
+  if(status != asynSuccess){
+    return status;
+  }
+
+
+  homing = 1;
+  next_poll= -1;
+  return status;
+}
+
+asynStatus NanotecAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
+{
+  asynStatus status;
+  //static const char *functionName = "NanotecAxis::moveVelocity";
+    double target;
+
+  // 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 */
+    pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&target);
+  } else {
+      /* This is a negative move  */
+    pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&target);
+  }
+
+  status = move(target,0,0,0,0);
+
+  return status;
+}
+
+asynStatus NanotecAxis::stop(double acceleration )
+{
+  asynStatus status;
+  //static const char *functionName = "NanotecAxis::stop";
+  char command[COMLEN], reply[COMLEN];
+
+  sprintf(command, "#%dS1", busAddress);
+  status = pC_->transactController(command,reply);
+  errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
+
+  return status;
+}
+
+asynStatus NanotecAxis::setPosition(double position)
+{
+  asynStatus status;
+  //static const char *functionName = "NanotecAxis::setPosition";
+  char command[COMLEN], reply[COMLEN];
+
+  sprintf(command, "#%dD%d", busAddress, (int)position);
+  status = pC_->transactController(command,reply);
+  next_poll = -1;
+
+  return status;
+}
+
+asynStatus NanotecAxis::setClosedLoop(bool closedLoop)
+{
+  //static const char *functionName = "NanotecAxis::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 NanotecAxis::poll(bool *moving)
+{ 
+  asynStatus comStatus;
+  char command[COMLEN], reply[COMLEN];
+  char *pPtr; 
+  int posVal, statVal;
+
+
+  // protect against excessive polling
+  if(time(NULL) < next_poll){
+    *moving = false;
+    return asynSuccess;
+  }
+ 
+  // Read the current motor position
+  sprintf(command,"#%dC", busAddress);
+  comStatus = pC_->transactController(command,reply);
+  if(comStatus) goto skip;
+
+  pPtr = strchr(reply,'C');
+  pPtr++;
+  posVal = atoi(pPtr);
+
+  errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal);
+  setDoubleParam(pC_->motorPosition_, (double)posVal);
+  //setDoubleParam(pC_->motorEncoderPosition_, position);
+  
+
+  // Read the moving status of this motor
+  sprintf(command,"#%d$",busAddress);
+  comStatus = pC_->transactController(command,reply);
+  if(comStatus) goto skip;
+
+  pPtr = strchr(reply,'$');
+  pPtr++;
+  statVal = atoi(pPtr);
+  errlogPrintf("Axis %d, reply %s, statVal = %d\n", 
+   	       axisNo_, reply, statVal);
+
+  setIntegerParam(pC_->motorStatusDone_, false);
+  *moving = true;
+  if(homing){
+    switch(statVal) {
+    case 164:
+      setPosition(.0);
+      break;
+    case 163:
+    case 161:
+      *moving = false;
+      setIntegerParam(pC_->motorStatusAtHome_, true);
+      setIntegerParam(pC_->motorStatusDone_, true);
+      break;
+    default :
+      if(statVal & 1) {
+	*moving = false;
+	setIntegerParam(pC_->motorStatusAtHome_, true);
+	setIntegerParam(pC_->motorStatusDone_, true);
+      }
+      break;
+    }
+  } else {
+      if(statVal & 1) {
+	*moving = false;
+	setIntegerParam(pC_->motorStatusDone_, true);
+      }
+  }
+
+  if(*moving == true){
+    next_poll = -1;
+  }
+
+  skip:
+  setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
+  callParamCallbacks();
+  return comStatus ? asynError : asynSuccess;
+}
+
+/** Code for iocsh registration */
+static const iocshArg NanotecCreateControllerArg0 = {"Port name", iocshArgString};
+static const iocshArg NanotecCreateControllerArg1 = {"Nanotec port name", iocshArgString};
+static const iocshArg NanotecCreateControllerArg2 = {"Number of axes", iocshArgInt};
+static const iocshArg NanotecCreateControllerArg3 = {"Komma separated list of bus addresses", iocshArgString};
+static const iocshArg * const NanotecCreateControllerArgs[] = {&NanotecCreateControllerArg0,
+							       &NanotecCreateControllerArg1,
+							       &NanotecCreateControllerArg2,
+							       &NanotecCreateControllerArg3
+                                                              };
+static const iocshFuncDef NanotecCreateControllerDef = {"NanotecCreateController", 4, NanotecCreateControllerArgs};
+static void NanotecCreateContollerCallFunc(const iocshArgBuf *args)
+{
+  NanotecCreateController(args[0].sval, args[1].sval, args[2].ival,args[3].sval);
+}
+
+static void NanotecRegister(void)
+{
+  iocshRegister(&NanotecCreateControllerDef, NanotecCreateContollerCallFunc);
+}
+
+extern "C" {
+epicsExportRegistrar(NanotecRegister);
+}