508 lines
14 KiB
C++
508 lines
14 KiB
C++
/*
|
|
FILENAME... NanotecDriver.cpp
|
|
USAGE... Motor driver support for the Nanotec SMCI controllers
|
|
connected to a RS-485 bus.
|
|
|
|
These motors sit on a bus. The address on the bus is a random number, not
|
|
necessarily the motor number. These asyn motor drivers do not have access to
|
|
the motor record. Thus it was impossible to pass the bus address through.
|
|
The option was to create an axiliary record but this sucked too because the
|
|
bus address a motor is an initialisation parameter and should not be visible
|
|
in the EPICS database. Thus I choose to pass the bus address as a comma separated
|
|
list to the motor controller constructor.
|
|
|
|
Mark Koennecke
|
|
July 2015
|
|
|
|
Modified to use the MsgTxt field for SINQ
|
|
|
|
Mark Koennecke, January 2019
|
|
*/
|
|
|
|
|
|
#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
|
|
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
|
|
/** 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)
|
|
: SINQController(portName, NanotecPortName, motCount+1)
|
|
{
|
|
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)
|
|
: SINQAxis(pC, axisNo),
|
|
pC_(pC)
|
|
{
|
|
this->busAddress = busAddress;
|
|
next_poll = -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 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(int axisNo, char command[COMLEN], char reply[COMLEN])
|
|
{
|
|
asynStatus status;
|
|
size_t in, out;
|
|
int reason;
|
|
SINQAxis *axis = getAxis(axisNo);
|
|
|
|
pasynOctetSyncIO->flush(pasynUserController_);
|
|
|
|
status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
|
|
reply,COMLEN, 1.,&out,&in,&reason);
|
|
if(status != asynSuccess){
|
|
if(axis != NULL){
|
|
axis->updateMsgTxtFromDriver("Lost connection to motor controller");
|
|
}
|
|
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;
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
// status = sendAccelAndVelocity(acceleration, maxVelocity);
|
|
|
|
if (relative) {
|
|
position += this->position;
|
|
}
|
|
|
|
homing = 0;
|
|
setIntegerParam(pC_->motorStatusAtHome_, false);
|
|
setIntegerParam(pC_->motorStatusHighLimit_, false);
|
|
setIntegerParam(pC_->motorStatusLowLimit_, false);
|
|
|
|
/*
|
|
set mode
|
|
*/
|
|
snprintf(command,sizeof(command),"#%dp2",busAddress);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(status != asynSuccess){
|
|
return status;
|
|
}
|
|
|
|
/*
|
|
set target
|
|
*/
|
|
snprintf(command,sizeof(command),"#%ds%d",busAddress, (int)position);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(status != asynSuccess){
|
|
return status;
|
|
}
|
|
|
|
/*
|
|
and start..
|
|
*/
|
|
snprintf(command,sizeof(command),"#%dA",busAddress);
|
|
status = pC_->transactController(axisNo_,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];
|
|
|
|
setIntegerParam(pC_->motorStatusAtHome_, false);
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
/*
|
|
reset positioning errors
|
|
*/
|
|
snprintf(command,sizeof(command),"#%dD",busAddress);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(status != asynSuccess){
|
|
return status;
|
|
}
|
|
|
|
/*
|
|
set mode
|
|
*/
|
|
snprintf(command,sizeof(command),"#%dp4",busAddress);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(status != asynSuccess){
|
|
return status;
|
|
}
|
|
|
|
|
|
/*
|
|
set direction
|
|
*/
|
|
snprintf(command,sizeof(command),"#%dd0",busAddress);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
if(status != asynSuccess){
|
|
return status;
|
|
}
|
|
|
|
|
|
/*
|
|
and start..
|
|
*/
|
|
snprintf(command,sizeof(command),"#%dA",busAddress);
|
|
status = pC_->transactController(axisNo_,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);
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
|
|
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(axisNo_,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];
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
sprintf(command, "#%dD%d", busAddress, (int)position);
|
|
status = pC_->transactController(axisNo_,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, count = 0;
|
|
double lowLim, highLim;
|
|
|
|
|
|
// 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(axisNo_,command,reply);
|
|
if(comStatus) goto skip;
|
|
|
|
pPtr = strchr(reply,'C');
|
|
if(pPtr){
|
|
pPtr++;
|
|
count = sscanf(pPtr,"%d", &posVal);
|
|
}
|
|
if(pPtr == NULL || count < 1) {
|
|
errlogPrintf("Invalid response %s for #C received for axis %d, address %d\n", reply, axisNo_, busAddress);
|
|
return asynError;
|
|
}
|
|
|
|
//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(axisNo_,command,reply);
|
|
if(comStatus) goto skip;
|
|
|
|
pPtr = strchr(reply,'$');
|
|
if(pPtr) {
|
|
pPtr++;
|
|
count = sscanf(pPtr, "%d", &statVal);
|
|
}
|
|
if(pPtr == NULL || count < 1) {
|
|
errlogPrintf("Invalid response %s for #$ received for axis %d busAddress %d\n", reply, axisNo_, busAddress);
|
|
return asynError;
|
|
}
|
|
//errlogPrintf("Axis %d, reply %s, statVal = %d\n",
|
|
// axisNo_, reply, statVal);
|
|
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
*moving = true;
|
|
pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&lowLim);
|
|
pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&highLim);
|
|
|
|
if(homing){
|
|
/*
|
|
code for homing
|
|
*/
|
|
switch(statVal) {
|
|
case 163:
|
|
setPosition(lowLim);
|
|
*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 {
|
|
/*
|
|
code for normal movement
|
|
*/
|
|
if(statVal & 1) {
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
} else if (statVal & 4) {
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
errlogSevPrintf(errlogMajor, "Limit or other positioning problem at %d", axisNo_);
|
|
updateMsgTxtFromDriver("Positioning problem");
|
|
if(ABS(posVal - lowLim) < ABS(posVal - highLim)){
|
|
setIntegerParam(pC_->motorStatusLowLimit_, true);
|
|
updateMsgTxtFromDriver("Low Limit Hit");
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
|
updateMsgTxtFromDriver("High Limit Hit");
|
|
}
|
|
*moving = false;
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|