680 lines
21 KiB
C++
680 lines
21 KiB
C++
/*
|
|
FILENAME... EuroMoveDriver.cpp
|
|
USAGE... Motor driver support for the EuroMove motor controller from LLB.
|
|
|
|
This is a driver for the EuroMove motor controller from LLB. This device talks to us either via
|
|
GPIB or USB. The controller is fairly simple. It can control quite a number of motors.
|
|
|
|
Reworked in 06/2021 to work with the Prologix GPIV-Ethernet converter
|
|
|
|
mark.koennecke@psi.ch
|
|
*/
|
|
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <ctype.h>
|
|
#include <time.h>
|
|
#include <unistd.h>
|
|
|
|
#include <iocsh.h>
|
|
#include <epicsThread.h>
|
|
#include <errlog.h>
|
|
|
|
#include <asynOctetSyncIO.h>
|
|
|
|
#include "EuroMoveDriver.h"
|
|
#include <epicsExport.h>
|
|
|
|
#define IDLEPOLL 60
|
|
|
|
/** Creates a new EuroMoveController object.
|
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
|
* \param[in] EuroMovePortName The name of the drvAsynSerialPort that was created previously to connect to the EuroMove controller
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
*/
|
|
EuroMoveController::EuroMoveController(const char *portName, const char *EuroMovePortName, int gpibAddr, int numAxis)
|
|
: SINQController(portName, EuroMovePortName,numAxis+1)
|
|
{
|
|
asynStatus status;
|
|
static const char *functionName = "EuroMoveController::EuroMoveController";
|
|
size_t out, in;
|
|
int reason;
|
|
char command[80], reply[80];
|
|
|
|
/* Connect to EuroMove controller */
|
|
status = pasynOctetSyncIO->connect(EuroMovePortName, 0, &pasynUserController_, NULL);
|
|
if (status) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: cannot connect to EuroMove controller\n",
|
|
functionName);
|
|
}
|
|
const char *terminator = "\r\n";
|
|
pasynOctetSyncIO->setOutputEos(pasynUserController_,terminator,strlen(terminator));
|
|
const char *repTerminator = "\r";
|
|
pasynOctetSyncIO->setInputEos(pasynUserController_,repTerminator,strlen(repTerminator));
|
|
|
|
/* Save gpib address and prepare addr string */
|
|
this->gpibAddr = gpibAddr;
|
|
snprintf(this->addrCommand,sizeof(addrCommand), "++addr %d", gpibAddr);
|
|
|
|
/*
|
|
Configure the Prologix interface to our liking
|
|
The thing sends no response on configuration commands
|
|
*/
|
|
strcpy(command,"++mode 1"); /* Make it a controller */
|
|
status = pasynOctetSyncIO->write(pasynUserController_, command, strlen(command), 2, &out);
|
|
strcpy(command,"++auto 1"); /* ask for response automatically */
|
|
status = pasynOctetSyncIO->write(pasynUserController_, command, strlen(command), 2, &out);
|
|
strcpy(command,"++eos 1"); /* CR */
|
|
status = pasynOctetSyncIO->write(pasynUserController_, command, strlen(command), 2, &out);
|
|
|
|
|
|
|
|
for(int i = 0; i < numAxis; i++){
|
|
new EuroMoveAxis(this, i+1);
|
|
}
|
|
startPoller(1000./1000., IDLEPOLL, 2);
|
|
}
|
|
|
|
|
|
/** Creates a new EuroMoveController 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] EuroMovePortName The name of the drvAsynIPPPort that was created previously to connect to the EuroMove controller
|
|
* \param[in] gpibAddr The address of the controller on the GPIB bus
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
*/
|
|
extern "C" int EuroMoveCreateController(const char *portName, const char *EuroMovePortName, const int gpibAddr, const int numAxis)
|
|
{
|
|
new EuroMoveController(portName, EuroMovePortName, gpibAddr, numAxis);
|
|
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 EuroMoveController::report(FILE *fp, int level)
|
|
{
|
|
fprintf(fp, "EuroMove motor driver %s, numAxes=%d\n",
|
|
this->portName, numAxes_);
|
|
|
|
// Call the base class method
|
|
asynMotorController::report(fp, level);
|
|
}
|
|
|
|
/** Returns a pointer to an EuroMoveAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
|
EuroMoveAxis* EuroMoveController::getAxis(asynUser *pasynUser)
|
|
{
|
|
return static_cast<EuroMoveAxis*>(asynMotorController::getAxis(pasynUser));
|
|
}
|
|
|
|
/** Returns a pointer to an EuroMoveAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] axisNo Axis index number. */
|
|
EuroMoveAxis* EuroMoveController::getAxis(int axisNo)
|
|
{
|
|
return static_cast<EuroMoveAxis*>(asynMotorController::getAxis(axisNo));
|
|
}
|
|
|
|
/**
|
|
* send a command to the EuroMove and read the reply. Do test for errors.
|
|
* \param[in] command The command to send
|
|
* \param[out] reply The controllers reply
|
|
*/
|
|
|
|
asynStatus EuroMoveController::transactController(int axisNo,char command[COMLEN], char reply[COMLEN])
|
|
{
|
|
asynStatus status;
|
|
SINQAxis *axis = getAxis(axisNo);
|
|
size_t out, in;
|
|
int reason, errCode, lCode;
|
|
char pDummy[50], lReply[50];
|
|
const char *lCommand = {"L"};
|
|
|
|
|
|
|
|
pasynOctetSyncIO->flush(pasynUserController_);
|
|
|
|
/* set address first */
|
|
status = pasynOctetSyncIO->write(pasynUserController_, this->addrCommand, strlen(this->addrCommand), 2, &out);
|
|
if(status != asynSuccess){
|
|
if(axis!= NULL){
|
|
axis->updateMsgTxtFromDriver("Lost connection to motor controller");
|
|
errlogPrintf("Lost connection to motor controller\n");
|
|
}
|
|
return status;
|
|
}
|
|
|
|
/* now the actual command */
|
|
status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
|
|
reply,sizeof(reply), 10.,&out,&in,&reason);
|
|
if(status != asynSuccess){
|
|
if(axis!= NULL){
|
|
axis->updateMsgTxtFromDriver("Lost connection to motor controller");
|
|
errlogPrintf("Lost connection to motor controller\n");
|
|
}
|
|
return status;
|
|
}
|
|
|
|
if(strstr(reply,"ERROR") != NULL){
|
|
sscanf((const char *)reply, "%s %d", pDummy, &errCode);
|
|
switch(errCode){
|
|
case 0:
|
|
axis->updateMsgTxtFromDriver("Syntax error or parameter out of range");
|
|
errlogPrintf("Syntax error or parameter out or range on %d\n", axisNo);
|
|
break;
|
|
case 1:
|
|
axis->updateMsgTxtFromDriver("Timeout communicating with daughter board");
|
|
errlogPrintf("Timeout communicating with daughter board on %d\n", axisNo);
|
|
break;
|
|
case 2:
|
|
axis->updateMsgTxtFromDriver("This is not a drivable motor");
|
|
errlogPrintf("This is not a drivable motor on %d\n", axisNo);
|
|
break;
|
|
case 3:
|
|
axis->updateMsgTxtFromDriver("Encoder anomaly");
|
|
errlogPrintf("Encoder anomaly on %d\n", axisNo);
|
|
break;
|
|
case 4:
|
|
axis->updateMsgTxtFromDriver("Attempt to move across limit switch");
|
|
errlogPrintf("Attempt to move across limit switch on %d\n", axisNo);
|
|
break;
|
|
case 5:
|
|
axis->updateMsgTxtFromDriver("Badly configured relay system");
|
|
errlogPrintf("Badly configured relay system on %d\n", axisNo);
|
|
break;
|
|
case 6:
|
|
axis->updateMsgTxtFromDriver("Non valid backup");
|
|
errlogPrintf("Invalid backup on %d\n", axisNo);
|
|
break;
|
|
case 7:
|
|
axis->updateMsgTxtFromDriver("Backup failed");
|
|
errlogPrintf("Backup failed on %d\n", axisNo);
|
|
break;
|
|
}
|
|
return asynError;
|
|
}
|
|
|
|
/*
|
|
Test system status
|
|
*/
|
|
status = pasynOctetSyncIO->writeRead(pasynUserController_, lCommand, strlen(lCommand),
|
|
lReply,sizeof(lReply), 1.,&out,&in,&reason);
|
|
if(status != asynSuccess){
|
|
if(axis!= NULL){
|
|
axis->updateMsgTxtFromDriver("Lost connection to motor controller");
|
|
errlogPrintf("Lost connection to motor controller\n");
|
|
}
|
|
return status;
|
|
}
|
|
sscanf(lReply, "%x", &lCode);
|
|
//errlogPrintf("System status returned %s, converted to %d\n", lReply, lCode);
|
|
if((lCode & 1) > 0){
|
|
axis->updateMsgTxtFromDriver("Syntax error or impossible to execute");
|
|
errlogPrintf("Syntax error or impossible to execute for %s on %d\n", command, axisNo);
|
|
return asynError;
|
|
}
|
|
|
|
|
|
return status;
|
|
}
|
|
|
|
|
|
asynStatus EuroMoveController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
|
|
{
|
|
int function = pasynUser->reason;
|
|
asynStatus status = asynError;
|
|
EuroMoveAxis *pAxis = NULL;
|
|
|
|
static const char *functionName = "EuroMoveController::writeFloat64";
|
|
|
|
|
|
pAxis = (EuroMoveAxis *)this->getAxis(pasynUser);
|
|
if (!pAxis) {
|
|
return asynError;
|
|
}
|
|
|
|
|
|
/* Set the parameter and readback in the parameter library. */
|
|
status = pAxis->setDoubleParam(function, value);
|
|
|
|
errlogPrintf("%s, reason %d, value %f\n", functionName, function, value);
|
|
|
|
// TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command
|
|
if (function == motorResolution_) {
|
|
if(value > .0){
|
|
pAxis->resolution = (int)(1./value);
|
|
} else {
|
|
pAxis->resolution = 1;
|
|
}
|
|
}
|
|
|
|
//Call base class method
|
|
//This will handle callCallbacks even if the function was handled here.
|
|
status = asynMotorController::writeFloat64(pasynUser, value);
|
|
|
|
return status;
|
|
|
|
}
|
|
|
|
// These are the EuroMoveAxis methods
|
|
|
|
/** Creates a new EuroMoveAxis object.
|
|
* \param[in] pC Pointer to the EuroMoveController to which this axis belongs.
|
|
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
|
*
|
|
* Initializes register numbers, etc.
|
|
*/
|
|
EuroMoveAxis::EuroMoveAxis(EuroMoveController *pC, int axisNo)
|
|
: SINQAxis(pC, axisNo),
|
|
pC_(pC)
|
|
{
|
|
motNo = axisNo;
|
|
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 EuroMoveAxis::report(FILE *fp, int level)
|
|
{
|
|
if (level > 0) {
|
|
fprintf(fp, " axis %d\n",
|
|
axisNo_);
|
|
}
|
|
asynMotorAxis::report(fp, level);
|
|
}
|
|
|
|
|
|
asynStatus EuroMoveAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "EuroMoveAxis::move";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
updateMsgTxtFromDriver("");
|
|
|
|
if (relative) {
|
|
position += this->position;
|
|
}
|
|
homingStatus = HomeIdle;
|
|
sprintf(command,"G%d=%d", motNo, (int)position);
|
|
status = pC_->transactController(motNo,command,reply);
|
|
next_poll = -1;
|
|
targetPosition = (int)position;
|
|
return status;
|
|
}
|
|
|
|
asynStatus EuroMoveAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
|
{
|
|
asynStatus status = asynSuccess;
|
|
//static const char *functionName = "EuroMoveAxis::home";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
/*
|
|
The EuroMove controller does not have a built in homing mechanism. We simulate it by:
|
|
|
|
1). Running the motor in the appropriate end swicth at high speed
|
|
2) Stepping back three units
|
|
3) Run the motor again in the end switch at low speed for better accuracy
|
|
4) Set the limit position with the <I> command
|
|
|
|
The state of this operation is maintained in the homingStatus variable
|
|
*/
|
|
homingDirection = forwards;
|
|
homingStatus = HomeFastRun;
|
|
if(homingDirection){
|
|
sprintf(command,"H%d=03", motNo);
|
|
} else {
|
|
sprintf(command,"H%d=07", motNo);
|
|
}
|
|
|
|
status = pC_->transactController(motNo,command,reply);
|
|
next_poll = -1;
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus EuroMoveAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
|
{
|
|
asynStatus status;
|
|
double target;
|
|
|
|
//static const char *functionName = "EuroMoveAxis::moveVelocity";
|
|
|
|
// 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);
|
|
next_poll = -1;
|
|
return status;
|
|
}
|
|
|
|
asynStatus EuroMoveAxis::sendStop()
|
|
{
|
|
asynStatus status;
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
sprintf(command, "B%d", motNo);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
return status;
|
|
}
|
|
|
|
asynStatus EuroMoveAxis::stop(double acceleration )
|
|
{
|
|
asynStatus status;
|
|
// static const char *functionName = "EuroMoveAxis::stop";
|
|
|
|
status = sendStop();
|
|
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
|
|
updateMsgTxtFromDriver("Axis interrupted");
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus EuroMoveAxis::setPosition(double position)
|
|
{
|
|
asynStatus status;
|
|
//static const char *functionName = "EuroMoveAxis::setPosition";
|
|
char command[COMLEN], reply[COMLEN];
|
|
|
|
errlogPrintf("EuroMoveAxis::setPosition called with %lf\n", position);
|
|
|
|
sprintf(command, "I%d=%d",motNo,(int)position);
|
|
status = pC_->transactController(axisNo_,command,reply);
|
|
next_poll = -1;
|
|
|
|
return status;
|
|
}
|
|
|
|
asynStatus EuroMoveAxis::setClosedLoop(bool closedLoop)
|
|
{
|
|
//static const char *functionName = "EuroMoveAxis::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 EuroMoveAxis::poll(bool *moving)
|
|
{
|
|
asynStatus comStatus = asynSuccess;
|
|
char command[COMLEN], reply[COMLEN];
|
|
unsigned int axStatus;
|
|
double backStep, backPos, limit;
|
|
|
|
// protect against excessive polling
|
|
if(time(NULL) < next_poll){
|
|
*moving = false;
|
|
return asynSuccess;
|
|
}
|
|
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
|
|
/*
|
|
read the current position
|
|
*/
|
|
sprintf(command,"A%d",motNo);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError) {
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
goto skip;
|
|
}
|
|
sscanf(reply,"%d",&position);
|
|
setDoubleParam(pC_->motorPosition_,(double)position);
|
|
|
|
// Read the moving status of this motor
|
|
sprintf(command,"E%d",motNo);
|
|
comStatus = pC_->transactController(axisNo_,command,reply);
|
|
if(comStatus == asynError){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
goto skip;
|
|
}
|
|
|
|
/* errlogPrintf("Axis %d, status reply %s, position %d\n", axisNo_, reply, position); */
|
|
sscanf(reply, "%x", &axStatus);
|
|
|
|
if(homingStatus == HomeIdle){
|
|
if((axStatus & 128) > 0) { // test bit 8
|
|
*moving = true;
|
|
next_poll = -1;
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
} else {
|
|
*moving = false;
|
|
next_poll = time(NULL)+IDLEPOLL;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
}
|
|
|
|
/* Testing limit switches */
|
|
if((axStatus & 2) > 0){ // bit 2
|
|
if(targetPosition > position) {
|
|
/*
|
|
Error codition only when we wish to move further
|
|
*/
|
|
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
|
errlogPrintf("HighLimit detected on %d\n", motNo);
|
|
updateMsgTxtFromDriver("On high limit switch");
|
|
sendStop();
|
|
}
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusHighLimit_, false);
|
|
}
|
|
if((axStatus & 1) > 0){ // bit 1
|
|
setIntegerParam(pC_->motorStatusLowLimit_, true);
|
|
if(targetPosition < position) {
|
|
/*
|
|
Error treatment onlly when we want to go below the limits
|
|
*/
|
|
errlogPrintf("LowLimit detected on %d\n", motNo);
|
|
sendStop();
|
|
updateMsgTxtFromDriver("On low limit switch");
|
|
}
|
|
} else {
|
|
setIntegerParam(pC_->motorStatusLowLimit_, false);
|
|
}
|
|
|
|
/* there could be errors reported in the motor status */
|
|
if((axStatus & 8) > 0){ // bit 4
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("Internal timeout detected");
|
|
errlogPrintf("Internal timeout detected on %d\n", motNo);
|
|
comStatus = asynError;
|
|
}
|
|
if((axStatus & 4) > 0){ // bit 3
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("Encoding anomaly");
|
|
errlogPrintf("Encoding anomaly on %d\n", motNo);
|
|
comStatus = asynError;
|
|
}
|
|
} else {
|
|
next_poll = -1;
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
*moving = true;
|
|
switch(homingStatus){
|
|
case HomeIdle:
|
|
// handled above: this is here to silence the compiler
|
|
break;
|
|
case HomeFastRun:
|
|
// errlogPrintf("HomeFastRun\n");
|
|
if(axStatus & 2 || axStatus & 1){
|
|
sendStop();
|
|
homingStatus = HomeFastStop;
|
|
}
|
|
break;
|
|
case HomeFastStop:
|
|
// errlogPrintf("HomeFastStop\n");
|
|
if((axStatus & 128) == 0){
|
|
backStep = 3 * resolution;
|
|
if(homingDirection){
|
|
backPos = position - backStep;
|
|
} else {
|
|
backPos = position + backStep;
|
|
}
|
|
sprintf(command,"G%d=%d", motNo, (int)backPos);
|
|
comStatus = pC_->transactController(motNo,command,reply);
|
|
if(comStatus == asynError){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("Homing problem when back stepping");
|
|
errlogPrintf("Homing problem when back stepping: %s not accepted\n", command);
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusAtHome_, true);
|
|
homingStatus = HomeIdle;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
goto skip;
|
|
}
|
|
usleep(200);
|
|
homingStatus = HomeBackStep;
|
|
}
|
|
break;
|
|
case HomeBackStep:
|
|
//errlogPrintf("HomeBackStep\n");
|
|
if((axStatus & 128) == 0){
|
|
homingStatus = HomeSlowRun;
|
|
if(homingDirection){
|
|
sprintf(command,"H%d=01", motNo);
|
|
} else {
|
|
sprintf(command,"H%d=05", motNo);
|
|
}
|
|
comStatus = pC_->transactController(motNo,command,reply);
|
|
if(comStatus == asynError){
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
updateMsgTxtFromDriver("Homing problem slow running");
|
|
errlogPrintf("Homing problem when slow running: %s not accepted\n", command);
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusAtHome_, true);
|
|
homingStatus = HomeIdle;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
goto skip;
|
|
}
|
|
}
|
|
break;
|
|
case HomeSlowRun:
|
|
//errlogPrintf("HomeSlowRun\n");
|
|
if(axStatus & 2 || axStatus & 1){
|
|
sendStop();
|
|
homingStatus = HomeSlowStop;
|
|
}
|
|
break;
|
|
case HomeSlowStop:
|
|
//errlogPrintf("HomeSlowStop\n");
|
|
if((axStatus & 128) == 0) {
|
|
if(homingDirection) {
|
|
pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&limit);
|
|
} else {
|
|
pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&limit);
|
|
}
|
|
setPosition(limit);
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
next_poll = time(NULL)+IDLEPOLL;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
setIntegerParam(pC_->motorStatusAtHome_, true);
|
|
homingStatus = HomeIdle;
|
|
setDoubleParam(pC_->motorPosition_,(double)limit);
|
|
}
|
|
}
|
|
}
|
|
|
|
skip:
|
|
callParamCallbacks();
|
|
return comStatus;
|
|
}
|
|
|
|
/** Code for configuring the motNo on axis **/
|
|
extern "C" {
|
|
asynStatus EuroMoveConfigureAxis(const char *euroMoveName, unsigned int axisNo, unsigned int motNo)
|
|
{
|
|
EuroMoveController *pC = NULL;
|
|
EuroMoveAxis *pAxis = NULL;
|
|
|
|
pC = (EuroMoveController *)findAsynPortDriver(euroMoveName);
|
|
if(!pC){
|
|
errlogPrintf("%s driver not found", euroMoveName);
|
|
return asynError;
|
|
}
|
|
pC->unlock();
|
|
|
|
pAxis = (EuroMoveAxis *)pC->getAxis(axisNo);
|
|
if(!pAxis){
|
|
errlogPrintf("axis %d driver not found on %s", axisNo, euroMoveName);
|
|
return asynError;
|
|
}
|
|
pAxis->setMotNo(motNo);
|
|
|
|
return asynSuccess;
|
|
}
|
|
}
|
|
|
|
static const iocshArg EuroMoveConfigArg0 = {"Port name", iocshArgString};
|
|
static const iocshArg EuroMoveConfigArg1 = {"Axis Index", iocshArgInt};
|
|
static const iocshArg EuroMoveConfigArg2 = {"Motor number in EuroMove", iocshArgInt};
|
|
static const iocshArg * const EuroMoveConfigArgs[] = {&EuroMoveConfigArg0,
|
|
&EuroMoveConfigArg1,
|
|
&EuroMoveConfigArg2
|
|
};
|
|
static const iocshFuncDef EuroMoveConfigDef = {"EuroMoveConfigureAxis", 3, EuroMoveConfigArgs};
|
|
static void EuroMoveConfigCallFunc(const iocshArgBuf *args)
|
|
{
|
|
EuroMoveConfigureAxis(args[0].sval, (unsigned int)args[1].ival, (unsigned int)args[2].ival);
|
|
}
|
|
|
|
|
|
/** Code for iocsh registration */
|
|
static const iocshArg EuroMoveCreateControllerArg0 = {"Port name", iocshArgString};
|
|
static const iocshArg EuroMoveCreateControllerArg1 = {"EuroMove port name", iocshArgString};
|
|
static const iocshArg EuroMoveCreateControllerArg2 = {"GPIB Address", iocshArgInt};
|
|
static const iocshArg EuroMoveCreateControllerArg3 = {"Number of axis", iocshArgInt};
|
|
static const iocshArg * const EuroMoveCreateControllerArgs[] = {&EuroMoveCreateControllerArg0,
|
|
&EuroMoveCreateControllerArg1,
|
|
&EuroMoveCreateControllerArg2,
|
|
&EuroMoveCreateControllerArg3
|
|
};
|
|
static const iocshFuncDef EuroMoveCreateControllerDef = {"EuroMoveCreateController", 4, EuroMoveCreateControllerArgs};
|
|
static void EuroMoveCreateControllerCallFunc(const iocshArgBuf *args)
|
|
{
|
|
EuroMoveCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival);
|
|
}
|
|
|
|
static void EuroMoveRegister(void)
|
|
{
|
|
iocshRegister(&EuroMoveCreateControllerDef, EuroMoveCreateControllerCallFunc);
|
|
iocshRegister(&EuroMoveConfigDef, EuroMoveConfigCallFunc);
|
|
}
|
|
|
|
extern "C" {
|
|
epicsExportRegistrar(EuroMoveRegister);
|
|
}
|