koennecke
af4d16d25f
with a motor axis. To this purpose a new derived class SINQAxis and SINQController were added which other drivers using this feature can derive from. The proof of concept and test platform is the EL734 driver.
460 lines
14 KiB
C++
460 lines
14 KiB
C++
/*
|
|
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)
|
|
: SINQController(portName, EL734PortName, numAxes)
|
|
{
|
|
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)
|
|
: SINQAxis(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);
|
|
if(status == asynError){
|
|
updateMsgTxtFromDriver(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);
|
|
if(status == asynError){
|
|
updateMsgTxtFromDriver(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);
|
|
if(status == asynError){
|
|
updateMsgTxtFromDriver(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_);
|
|
updateMsgTxtFromDriver("Axis interrupted");
|
|
|
|
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);
|
|
if(status == asynError){
|
|
updateMsgTxtFromDriver(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];
|
|
int driverError = 0;
|
|
|
|
|
|
// 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 == asynError){
|
|
updateMsgTxtFromDriver(reply);
|
|
driverError = 1;
|
|
}
|
|
if(comStatus) goto skip;
|
|
if(strstr(reply,"*ES") != NULL){
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("Emergency Stop Engaged");
|
|
driverError = 1;
|
|
goto skip;
|
|
} else if(strstr(reply,"?BSY") != NULL){
|
|
*moving = true;
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
updateMsgTxtFromDriver(NULL);
|
|
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);
|
|
updateMsgTxtFromDriver("Lower Limit Hit");
|
|
driverError = 1;
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusLowLimit_, false);
|
|
}
|
|
if(oredMSR & 0x20){
|
|
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
|
updateMsgTxtFromDriver("Upper Limit Hit");
|
|
driverError = 1;
|
|
} 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_);
|
|
updateMsgTxtFromDriver("Air cushion error");
|
|
driverError = 1;
|
|
}
|
|
if(oredMSR &0x80){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
errlogSevPrintf(errlogMajor, "Positioning fault at %d", axisNo_);
|
|
updateMsgTxtFromDriver("Positioning fault");
|
|
driverError = 1;
|
|
}
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
//updateMsgTxtFromDriver("Believed to be on position");
|
|
} else {
|
|
*moving = true;
|
|
next_poll = -1;
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
//updateMsgTxtFromDriver("Creeping");
|
|
}
|
|
|
|
skip:
|
|
if(driverError == 0){
|
|
updateMsgTxtFromDriver(NULL);
|
|
}
|
|
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);
|
|
}
|