lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

The member variable oredMSR of EL734Axis contains the axis status from the previous poll.

Browse Source 
Therefore, it needs to initialized with a sensible value (i.e. 1 which
means that the axis is standing still and has no errors.
This commit is contained in:
mathis_s
2024-09-10 08:58:59 +02:00
parent c972cce072
commit a6a8f14b26
2 changed files with 257 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

438
sinqEPICSApp/src/EL734Driver.cpp
View File

@ -11,7 +11,6 @@ Mark Koennecke, May, August 2017
*/ */
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <stdlib.h> #include <stdlib.h>
@ -30,84 +29,82 @@ Mark Koennecke, May, August 2017
#define IDLEPOLL 60 #define IDLEPOLL 60
/** Creates a new EL734Controller object. /** Creates a new EL734Controller object.
* \param[in] portName The name of the asyn port that will be created for this driver * \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] 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 * \param[in] numAxes The number of axes that this controller supports
*/ */
EL734Controller::EL734Controller(const char *portName, const char *EL734PortName, int numAxes) EL734Controller::EL734Controller(const char *portName, const char *EL734PortName, int numAxes)
: SINQController(portName, EL734PortName, numAxes) : SINQController(portName, EL734PortName, numAxes)
{ {
int axis; int axis;
asynStatus status; asynStatus status;
static const char *functionName = "EL734Controller::EL734Controller"; static const char *functionName = "EL734Controller::EL734Controller";
/* Connect to EL734 controller */ /* Connect to EL734 controller */
status = pasynOctetSyncIO->connect(EL734PortName, 0, &pasynUserController_, NULL); status = pasynOctetSyncIO->connect(EL734PortName, 0, &pasynUserController_, NULL);
if (status) { if (status)
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, {
"%s: cannot connect to EL734 controller\n", asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
functionName); "%s: cannot connect to EL734 controller\n",
functionName);
} }
pasynOctetSyncIO->setOutputEos(pasynUserController_,"\r",strlen("\r")); pasynOctetSyncIO->setOutputEos(pasynUserController_, "\r", strlen("\r"));
pasynOctetSyncIO->setInputEos(pasynUserController_,"\r",strlen("\r")); pasynOctetSyncIO->setInputEos(pasynUserController_, "\r", strlen("\r"));
switchRemote(); switchRemote();
for (axis=0; axis<numAxes; axis++) { for (axis = 0; axis < numAxes; axis++)
new EL734Axis(this, axis+1); {
new EL734Axis(this, axis + 1);
} }
startPoller(1000./1000., IDLEPOLL, 2); startPoller(1000. / 1000., IDLEPOLL, 2);
} }
/** Creates a new EL734Controller object. /** Creates a new EL734Controller object.
* Configuration command, called directly or from iocsh * 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] 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] 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 * \param[in] numAxes The number of axes that this controller supports
*/ */
extern "C" int EL734CreateController(const char *portName, const char *EL734PortName, int numAxes) extern "C" int EL734CreateController(const char *portName, const char *EL734PortName, int numAxes)
{ {
EL734Controller *pEL734Controller EL734Controller *pEL734Controller = new EL734Controller(portName, EL734PortName, numAxes);
= new EL734Controller(portName, EL734PortName, numAxes);
pEL734Controller = NULL; pEL734Controller = NULL;
return(asynSuccess); return (asynSuccess);
} }
/** Reports on status of the driver /** Reports on status of the driver
* \param[in] fp The file pointer on which report information will be written * \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired * \param[in] level The level of report detail desired
* *
* If details > 0 then information is printed about each axis. * If details > 0 then information is printed about each axis.
* After printing controller-specific information it calls asynMotorController::report() * After printing controller-specific information it calls asynMotorController::report()
*/ */
void EL734Controller::report(FILE *fp, int level) void EL734Controller::report(FILE *fp, int level)
{ {
fprintf(fp, "EL734 motor driver %s, numAxes=%d\n", fprintf(fp, "EL734 motor driver %s, numAxes=%d\n",
this->portName, numAxes_); this->portName, numAxes_);
// Call the base class method // Call the base class method
asynMotorController::report(fp, level); asynMotorController::report(fp, level);
} }
/** Returns a pointer to an EL734Axis object. /** Returns a pointer to an EL734Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid. * Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */ * \param[in] pasynUser asynUser structure that encodes the axis index number. */
EL734Axis* EL734Controller::getAxis(asynUser *pasynUser) EL734Axis *EL734Controller::getAxis(asynUser *pasynUser)
{ {
return static_cast<EL734Axis*>(asynMotorController::getAxis(pasynUser)); return static_cast<EL734Axis *>(asynMotorController::getAxis(pasynUser));
} }
/** Returns a pointer to an EL734Axis object. /** Returns a pointer to an EL734Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid. * Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] axisNo Axis index number. */ * \param[in] axisNo Axis index number. */
EL734Axis* EL734Controller::getAxis(int axisNo) EL734Axis *EL734Controller::getAxis(int axisNo)
{ {
return static_cast<EL734Axis*>(asynMotorController::getAxis(axisNo)); return static_cast<EL734Axis *>(asynMotorController::getAxis(axisNo));
} }
void EL734Controller::switchRemote() void EL734Controller::switchRemote()
@ -116,27 +113,27 @@ void EL734Controller::switchRemote()
size_t in, out; size_t in, out;
int reason; int reason;
strcpy(command,"RMT 1"); strcpy(command, "RMT 1");
pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 1.,&out,&in,&reason); reply, COMLEN, 1., &out, &in, &reason);
strcpy(command,"ECHO 0"); strcpy(command, "ECHO 0");
pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 1.,&out,&in,&reason); reply, COMLEN, 1., &out, &in, &reason);
strcpy(command,"RMT 1"); strcpy(command, "RMT 1");
pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 1.,&out,&in,&reason); reply, COMLEN, 1., &out, &in, &reason);
strcpy(command,"ECHO 0"); strcpy(command, "ECHO 0");
pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 1.,&out,&in,&reason); reply, COMLEN, 1., &out, &in, &reason);
} }
/** /**
* send a command to the EL734 and read the reply. Do some error and controller * send a command to the EL734 and read the reply. Do some error and controller
* issue fixing on the way * issue fixing on the way
* \param[in] command The command to send * \param[in] command The command to send
* \param[out] reply The controllers reply * \param[out] reply The controllers reply
*/ */
asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN], char reply[COMLEN]) asynStatus EL734Controller::transactController(int axisNo, char command[COMLEN], char reply[COMLEN])
{ {
asynStatus status; asynStatus status;
size_t in, out, i; size_t in, out, i;
@ -145,58 +142,71 @@ asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN],
SINQAxis *axis = getAxis(axisNo); SINQAxis *axis = getAxis(axisNo);
pasynOctetSyncIO->flush(pasynUserController_); pasynOctetSyncIO->flush(pasynUserController_);
status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 2.,&out,&in,&reason); reply, COMLEN, 2., &out, &in, &reason);
if(status != asynSuccess){ if (status != asynSuccess)
if(axis!= NULL){ {
if (axis != NULL)
{
axis->updateMsgTxtFromDriver("Lost connection to motor controller"); axis->updateMsgTxtFromDriver("Lost connection to motor controller");
} }
return status; return status;
} }
/* /*
check for the offline reply check for the offline reply
*/ */
if(strstr(reply,"?LOC") != NULL){ if (strstr(reply, "?LOC") != NULL)
{
switchRemote(); switchRemote();
return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 1.,&out,&in,&reason); reply, COMLEN, 1., &out, &in, &reason);
} }
/* /*
check for echos. This means that the thing is offline. check for echos. This means that the thing is offline.
*/ */
if(strstr(reply,"p ") != NULL || strstr(reply,"u ") != NULL || strstr(reply,"msr ") != NULL){ if (strstr(reply, "p ") != NULL || strstr(reply, "u ") != NULL || strstr(reply, "msr ") != NULL)
{
switchRemote(); switchRemote();
return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
reply,COMLEN, 1.,&out,&in,&reason); reply, COMLEN, 1., &out, &in, &reason);
} }
/* /*
check for EL734 errors check for EL734 errors
*/ */
strcpy(myReply, reply); strcpy(myReply, reply);
for(i = 0; i < strlen(reply); i++){ for (i = 0; i < strlen(reply); i++)
{
myReply[i] = (char)tolower((int)reply[i]); myReply[i] = (char)tolower((int)reply[i]);
} }
if(strstr(myReply,"?cmd") != NULL){ if (strstr(myReply, "?cmd") != NULL)
snprintf(errTxt,sizeof(errTxt), "Bad command %s at axis %d", command, axisNo); {
snprintf(errTxt, sizeof(errTxt), "Bad command %s at axis %d", command, axisNo);
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt);
if(axis!= NULL){ if (axis != NULL)
{
axis->updateMsgTxtFromDriver(errTxt); axis->updateMsgTxtFromDriver(errTxt);
} }
return asynError; return asynError;
} else if(strstr(myReply,"?par") != NULL){ }
snprintf(errTxt,sizeof(errTxt), "Bad parameter in command %s", command); else if (strstr(myReply, "?par") != NULL)
{
snprintf(errTxt, sizeof(errTxt), "Bad parameter in command %s", command);
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt);
if(axis!= NULL){ if (axis != NULL)
{
axis->updateMsgTxtFromDriver(errTxt); axis->updateMsgTxtFromDriver(errTxt);
} }
return asynError; return asynError;
} else if(strstr(myReply,"?rng") != NULL){ }
snprintf(errTxt,sizeof(errTxt), "Parameter out of range in command %s", command); else if (strstr(myReply, "?rng") != NULL)
{
snprintf(errTxt, sizeof(errTxt), "Parameter out of range in command %s", command);
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt); asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, errTxt);
if(axis!= NULL){ if (axis != NULL)
{
axis->updateMsgTxtFromDriver(errTxt); axis->updateMsgTxtFromDriver(errTxt);
} }
return asynError; return asynError;
@ -208,14 +218,14 @@ asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN],
// These are the EL734Axis methods // These are the EL734Axis methods
/** Creates a new EL734Axis object. /** Creates a new EL734Axis object.
* \param[in] pC Pointer to the EL734Controller to which this axis belongs. * \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. * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
* *
* Initializes register numbers, etc. * Initializes register numbers, etc.
*/ */
EL734Axis::EL734Axis(EL734Controller *pC, int axisNo) EL734Axis::EL734Axis(EL734Controller *pC, int axisNo)
: SINQAxis(pC, axisNo), pC_(pC) : SINQAxis(pC, axisNo), pC_(pC)
{ {
char command[COMLEN], reply[COMLEN]; char command[COMLEN], reply[COMLEN];
asynStatus status; asynStatus status;
int count = 0; int count = 0;
@ -224,52 +234,62 @@ EL734Axis::EL734Axis(EL734Controller *pC, int axisNo)
/* /*
get the hardware limits from the controller get the hardware limits from the controller
*/ */
sprintf(command,"H %d",axisNo_); sprintf(command, "H %d", axisNo_);
status = pC_->transactController(axisNo_,command,reply); status = pC_->transactController(axisNo_, command, reply);
if(status == asynSuccess){ if (status == asynSuccess)
count = sscanf(reply,"%f %f",&low, &high); {
if(count >= 2){ count = sscanf(reply, "%f %f", &low, &high);
pC_->setDoubleParam(axisNo_,pC_->motorLowLimit_,low); if (count >= 2)
pC_->setDoubleParam(axisNo_,pC_->motorHighLimit_,high); {
callParamCallbacks(); pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, low);
} else { pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, high);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, callParamCallbacks();
"Bad response - %s - requesting limits at axis %d", reply, axisNo_); }
else
{
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Bad response - %s - requesting limits at axis %d", reply, axisNo_);
} }
} else {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Failed to read limits at axis %d", axisNo_);
} }
else
{
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Failed to read limits at axis %d", axisNo_);
}
/*
oredMSR contains the axis status from the previous poll. The initial value
1 means that the axis is not moving and has no errors.
*/
oredMSR = 1;
} }
/** Reports on status of the axis /** Reports on status of the axis
* \param[in] fp The file pointer on which report information will be written * \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired * \param[in] level The level of report detail desired
* *
* After printing device-specific information calls asynMotorAxis::report() * After printing device-specific information calls asynMotorAxis::report()
*/ */
void EL734Axis::report(FILE *fp, int level) void EL734Axis::report(FILE *fp, int level)
{ {
if (level > 0) { if (level > 0)
{
fprintf(fp, " axis %d\n", fprintf(fp, " axis %d\n",
axisNo_); axisNo_);
} }
// Call the base class method // Call the base class method
//asynMotorAxis::report(fp, level); // asynMotorAxis::report(fp, level);
} }
asynStatus EL734Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) asynStatus EL734Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{ {
asynStatus status; asynStatus status;
//static const char *functionName = "EL734Axis::move"; // static const char *functionName = "EL734Axis::move";
char command[COMLEN], reply[COMLEN]; char command[COMLEN], reply[COMLEN];
// status = sendAccelAndVelocity(acceleration, maxVelocity); // status = sendAccelAndVelocity(acceleration, maxVelocity);
errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity); errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity);
/* /*
@ -278,16 +298,16 @@ asynStatus EL734Axis::move(double position, int relative, double minVelocity, do
sprintf(command, "J %d %d", axisNo_, (int)maxVelocity); sprintf(command, "J %d %d", axisNo_, (int)maxVelocity);
status = pC_->transactController(axisNo_, command, reply); status = pC_->transactController(axisNo_, command, reply);
if (relative) { if (relative)
{
position += this->position; position += this->position;
} }
oredMSR = 0;
homing = 0; homing = 0;
errorReported = 0; errorReported = 0;
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Starting axis %d with destination %f", axisNo_,position/1000); "Starting axis %d with destination %f", axisNo_, position / 1000);
sprintf(command, "p %d %.3f", axisNo_, position/1000.); sprintf(command, "p %d %.3f", axisNo_, position / 1000.);
status = pC_->transactController(axisNo_,command,reply); status = pC_->transactController(axisNo_, command, reply);
setIntegerParam(pC_->motorStatusProblem_, false); setIntegerParam(pC_->motorStatusProblem_, false);
updateMsgTxtFromDriver(""); updateMsgTxtFromDriver("");
next_poll = -1; next_poll = -1;
@ -297,7 +317,7 @@ asynStatus EL734Axis::move(double position, int relative, double minVelocity, do
asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{ {
asynStatus status; asynStatus status;
//static const char *functionName = "EL734Axis::home"; // static const char *functionName = "EL734Axis::home";
char command[COMLEN], reply[COMLEN]; char command[COMLEN], reply[COMLEN];
// status = sendAccelAndVelocity(acceleration, maxVelocity); // status = sendAccelAndVelocity(acceleration, maxVelocity);
@ -308,52 +328,55 @@ asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double accele
sprintf(command, "R %d", axisNo_); sprintf(command, "R %d", axisNo_);
homing = 1; homing = 1;
next_poll= -1; next_poll = -1;
status = pC_->transactController(axisNo_,command,reply); status = pC_->transactController(axisNo_, command, reply);
return status; return status;
} }
asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
{ {
asynStatus status; asynStatus status;
//static const char *functionName = "EL734Axis::moveVelocity"; // static const char *functionName = "EL734Axis::moveVelocity";
char command[COMLEN], reply[COMLEN]; char command[COMLEN], reply[COMLEN];
// asynPrint(pasynUser_, ASYN_TRACE_FLOW, // asynPrint(pasynUser_, ASYN_TRACE_FLOW,
// "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", // "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
// functionName, minVelocity, maxVelocity, acceleration); // functionName, minVelocity, maxVelocity, acceleration);
errorReported = 0; errorReported = 0;
if (maxVelocity > 0.) { if (maxVelocity > 0.)
{
/* This is a positive move */ /* This is a positive move */
sprintf(command, "FF %d", axisNo_); sprintf(command, "FF %d", axisNo_);
} else {
/* This is a negative move */
sprintf(command, "FB %d", axisNo_);
} }
status = pC_->transactController(axisNo_,command,reply); else
{
/* This is a negative move */
sprintf(command, "FB %d", axisNo_);
}
status = pC_->transactController(axisNo_, command, reply);
setIntegerParam(pC_->motorStatusProblem_, false); setIntegerParam(pC_->motorStatusProblem_, false);
updateMsgTxtFromDriver(""); updateMsgTxtFromDriver("");
next_poll = -1; next_poll = -1;
return status; return status;
} }
asynStatus EL734Axis::stop(double acceleration ) asynStatus EL734Axis::stop(double acceleration)
{ {
asynStatus status = asynSuccess; asynStatus status = asynSuccess;
//static const char *functionName = "EL734Axis::stop"; // static const char *functionName = "EL734Axis::stop";
char command[COMLEN], reply[COMLEN]; char command[COMLEN], reply[COMLEN];
bool moving = false; bool moving = false;
this->poll(&moving); this->poll(&moving);
if(moving && errorReported == 0){ if (moving && errorReported == 0)
sprintf(command, "S %d", axisNo_); {
status = pC_->transactController(axisNo_,command,reply); sprintf(command, "S %d", axisNo_);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Sent STOP on Axis %d\n", axisNo_); status = pC_->transactController(axisNo_, command, reply);
updateMsgTxtFromDriver("Axis interrupted"); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Sent STOP on Axis %d\n", axisNo_);
errorReported = 1; updateMsgTxtFromDriver("Axis interrupted");
} errorReported = 1;
}
return status; return status;
} }
@ -361,11 +384,11 @@ asynStatus EL734Axis::stop(double acceleration )
asynStatus EL734Axis::setPosition(double position) asynStatus EL734Axis::setPosition(double position)
{ {
asynStatus status; asynStatus status;
//static const char *functionName = "EL734Axis::setPosition"; // static const char *functionName = "EL734Axis::setPosition";
char command[COMLEN], reply[COMLEN]; char command[COMLEN], reply[COMLEN];
sprintf(command, "U %d %f", axisNo_, position/1000.); sprintf(command, "U %d %f", axisNo_, position / 1000.);
status = pC_->transactController(axisNo_,command,reply); status = pC_->transactController(axisNo_, command, reply);
next_poll = -1; next_poll = -1;
return status; return status;
@ -373,8 +396,8 @@ asynStatus EL734Axis::setPosition(double position)
asynStatus EL734Axis::setClosedLoop(bool closedLoop) asynStatus EL734Axis::setClosedLoop(bool closedLoop)
{ {
//static const char *functionName = "EL734Axis::setClosedLoop"; // static const char *functionName = "EL734Axis::setClosedLoop";
/* /*
This belongs into the Kingdom of Electronics. This belongs into the Kingdom of Electronics.
We do not do this. We do not do this.
@ -384,45 +407,50 @@ asynStatus EL734Axis::setClosedLoop(bool closedLoop)
} }
/** Polls the axis. /** Polls the axis.
* This function reads the motor position, the limit status, the home status, the moving status, * This function reads the motor position, the limit status, the home status, the moving status,
* and the drive power-on status. * and the drive power-on status.
* It calls setIntegerParam() and setDoubleParam() for each item that it polls, * It calls setIntegerParam() and setDoubleParam() for each item that it polls,
* and then calls callParamCallbacks() at the end. * 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). */ * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus EL734Axis::poll(bool *moving) asynStatus EL734Axis::poll(bool *moving)
{ {
int msr, count; int msr, count;
float low, high; float low, high;
asynStatus comStatus = asynSuccess; asynStatus comStatus = asynSuccess;
char command[COMLEN], reply[COMLEN], errTxt[256]; char command[COMLEN], reply[COMLEN], errTxt[256];
// protect against excessive polling // protect against excessive polling
if(time(NULL) < next_poll){ if (time(NULL) < next_poll)
{
*moving = false; *moving = false;
return asynSuccess; return asynSuccess;
} }
// read hardware limits // read hardware limits
sprintf(command,"H %d",axisNo_); sprintf(command, "H %d", axisNo_);
comStatus = pC_->transactController(axisNo_,command,reply); comStatus = pC_->transactController(axisNo_, command, reply);
if(comStatus == asynSuccess){ if (comStatus == asynSuccess)
count = sscanf(reply,"%f %f",&low, &high); {
if(count >= 2){ count = sscanf(reply, "%f %f", &low, &high);
pC_->setDoubleParam(axisNo_,pC_->motorLowLimit_,low); if (count >= 2)
pC_->setDoubleParam(axisNo_,pC_->motorHighLimit_,high); {
callParamCallbacks(); pC_->setDoubleParam(axisNo_, pC_->motorLowLimit_, low);
pC_->setDoubleParam(axisNo_, pC_->motorHighLimit_, high);
callParamCallbacks();
} }
} }
// Read the current motor position // Read the current motor position
setIntegerParam(pC_->motorStatusProblem_,false); setIntegerParam(pC_->motorStatusProblem_, false);
sprintf(command,"u %d", axisNo_); sprintf(command, "u %d", axisNo_);
comStatus = pC_->transactController(axisNo_,command,reply); comStatus = pC_->transactController(axisNo_, command, reply);
if(comStatus == asynError){ if (comStatus == asynError)
setIntegerParam(pC_->motorStatusProblem_,true); {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip; goto skip;
} }
if(strstr(reply,"*ES") != NULL){ if (strstr(reply, "*ES") != NULL)
{
*moving = false; *moving = false;
setIntegerParam(pC_->motorStatusDone_, true); setIntegerParam(pC_->motorStatusDone_, true);
setIntegerParam(pC_->motorStatusProblem_, true); setIntegerParam(pC_->motorStatusProblem_, true);
@ -430,70 +458,86 @@ asynStatus EL734Axis::poll(bool *moving)
updateMsgTxtFromDriver("Emergency Stop Engaged"); updateMsgTxtFromDriver("Emergency Stop Engaged");
comStatus = asynError; comStatus = asynError;
goto skip; goto skip;
} else if(strstr(reply,"?BSY") != NULL){ }
else if (strstr(reply, "?BSY") != NULL)
{
*moving = true; *moving = true;
setIntegerParam(pC_->motorStatusDone_, false); setIntegerParam(pC_->motorStatusDone_, false);
goto skip; goto skip;
} }
count = sscanf(reply,"%lf", &position); count = sscanf(reply, "%lf", &position);
if(count != 1) { if (count != 1)
if(!homing) { {
snprintf(errTxt,sizeof(errTxt),"Bad reply %s when reading position for %d", reply, axisNo_); if (!homing)
{
snprintf(errTxt, sizeof(errTxt), "Bad reply %s when reading position for %d", reply, axisNo_);
setIntegerParam(pC_->motorStatusProblem_, true); setIntegerParam(pC_->motorStatusProblem_, true);
errorReported = 1; errorReported = 1;
updateMsgTxtFromDriver(errTxt); updateMsgTxtFromDriver(errTxt);
comStatus = asynError; comStatus = asynError;
goto skip; goto skip;
} }
} else { }
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, else
"Axis %d, reply %s, position %lf\n", axisNo_, reply, position); {
setDoubleParam(pC_->motorPosition_, position*1000); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
//setDoubleParam(pC_->motorEncoderPosition_, position); "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 // Read the moving status of this motor
sprintf(command,"msr %d",axisNo_); sprintf(command, "msr %d", axisNo_);
comStatus = pC_->transactController(axisNo_,command,reply); comStatus = pC_->transactController(axisNo_, command, reply);
if(comStatus == asynError){ if (comStatus == asynError)
setIntegerParam(pC_->motorStatusProblem_,true); {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip; goto skip;
} }
sscanf(reply,"%x",&msr); sscanf(reply, "%x", &msr);
//errlogPrintf("Axis %d, reply %s, msr %d, oredmsr = %d, position = %lf\n", // errlogPrintf("Axis %d, reply %s, msr %d, oredmsr = %d, position = %lf\n",
// axisNo_, reply, msr, oredMSR, position); // axisNo_, reply, msr, oredMSR, position);
oredMSR |= msr; oredMSR |= msr;
if( (msr & 0x1) == 0){ if ((msr & 0x1) == 0)
{
// done: check for trouble // done: check for trouble
//errlogPrintf("Axis %d finished\n", axisNo_); // errlogPrintf("Axis %d finished\n", axisNo_);
*moving = false; *moving = false;
setIntegerParam(pC_->motorStatusDone_, true); setIntegerParam(pC_->motorStatusDone_, true);
next_poll = time(NULL)+IDLEPOLL; next_poll = time(NULL) + IDLEPOLL;
if(oredMSR & 0x10){ if (oredMSR & 0x10)
{
setIntegerParam(pC_->motorStatusLowLimit_, true); setIntegerParam(pC_->motorStatusLowLimit_, true);
updateMsgTxtFromDriver("Lower Limit Hit"); updateMsgTxtFromDriver("Lower Limit Hit");
errorReported = 1; errorReported = 1;
comStatus = asynError; comStatus = asynError;
goto skip; goto skip;
} else { }
else
{
setIntegerParam(pC_->motorStatusLowLimit_, false); setIntegerParam(pC_->motorStatusLowLimit_, false);
} }
if(oredMSR & 0x20){ if (oredMSR & 0x20)
{
setIntegerParam(pC_->motorStatusHighLimit_, true); setIntegerParam(pC_->motorStatusHighLimit_, true);
updateMsgTxtFromDriver("Upper Limit Hit"); updateMsgTxtFromDriver("Upper Limit Hit");
errorReported = 1; errorReported = 1;
comStatus = asynError; comStatus = asynError;
goto skip; goto skip;
} else { }
else
{
setIntegerParam(pC_->motorStatusHighLimit_, false); setIntegerParam(pC_->motorStatusHighLimit_, false);
} }
if(homing){ if (homing)
{
setIntegerParam(pC_->motorStatusAtHome_, true); setIntegerParam(pC_->motorStatusAtHome_, true);
} }
if(oredMSR & 0x1000){ if (oredMSR & 0x1000)
{
setIntegerParam(pC_->motorStatusProblem_, true); setIntegerParam(pC_->motorStatusProblem_, true);
// errlogPrintf("Detected air cushion error on %d", axisNo_); // errlogPrintf("Detected air cushion error on %d", axisNo_);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Air cushion problem on %d", axisNo_); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Air cushion problem on %d", axisNo_);
@ -502,7 +546,8 @@ asynStatus EL734Axis::poll(bool *moving)
comStatus = asynError; comStatus = asynError;
goto skip; goto skip;
} }
if(oredMSR & 0x100){ if (oredMSR & 0x100)
{
setIntegerParam(pC_->motorStatusProblem_, true); setIntegerParam(pC_->motorStatusProblem_, true);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Run failure at %d", axisNo_); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Run failure at %d", axisNo_);
updateMsgTxtFromDriver("Run failure"); updateMsgTxtFromDriver("Run failure");
@ -510,7 +555,8 @@ asynStatus EL734Axis::poll(bool *moving)
errorReported = 1; errorReported = 1;
goto skip; goto skip;
} }
if(oredMSR & 0x400){ if (oredMSR & 0x400)
{
setIntegerParam(pC_->motorStatusProblem_, true); setIntegerParam(pC_->motorStatusProblem_, true);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Positioning failure at %d", axisNo_); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Positioning failure at %d", axisNo_);
updateMsgTxtFromDriver("Positioning failure"); updateMsgTxtFromDriver("Positioning failure");
@ -518,17 +564,20 @@ asynStatus EL734Axis::poll(bool *moving)
errorReported = 1; errorReported = 1;
goto skip; goto skip;
} }
if(oredMSR & 0x200 || oredMSR & 0x80) { if (oredMSR & 0x200 || oredMSR & 0x80)
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, "Positioning fault at %d", axisNo_); {
} asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, "Positioning fault at %d", axisNo_);
}
setIntegerParam(pC_->motorStatusProblem_, false); setIntegerParam(pC_->motorStatusProblem_, false);
} else { }
else
{
*moving = true; *moving = true;
next_poll = -1; next_poll = -1;
setIntegerParam(pC_->motorStatusDone_, false); setIntegerParam(pC_->motorStatusDone_, false);
} }
skip: skip:
callParamCallbacks(); callParamCallbacks();
return comStatus; return comStatus;
} }
@ -537,9 +586,9 @@ asynStatus EL734Axis::poll(bool *moving)
static const iocshArg EL734CreateControllerArg0 = {"Port name", iocshArgString}; static const iocshArg EL734CreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg EL734CreateControllerArg1 = {"EL734 port name", iocshArgString}; static const iocshArg EL734CreateControllerArg1 = {"EL734 port name", iocshArgString};
static const iocshArg EL734CreateControllerArg2 = {"Number of axes", iocshArgInt}; static const iocshArg EL734CreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg * const EL734CreateControllerArgs[] = {&EL734CreateControllerArg0, static const iocshArg *const EL734CreateControllerArgs[] = {&EL734CreateControllerArg0,
&EL734CreateControllerArg1, &EL734CreateControllerArg1,
&EL734CreateControllerArg2}; &EL734CreateControllerArg2};
static const iocshFuncDef EL734CreateControllerDef = {"EL734CreateController", 3, EL734CreateControllerArgs}; static const iocshFuncDef EL734CreateControllerDef = {"EL734CreateController", 3, EL734CreateControllerArgs};
static void EL734CreateContollerCallFunc(const iocshArgBuf *args) static void EL734CreateContollerCallFunc(const iocshArgBuf *args)
{ {
@ -551,6 +600,7 @@ static void EL734Register(void)
iocshRegister(&EL734CreateControllerDef, EL734CreateContollerCallFunc); iocshRegister(&EL734CreateControllerDef, EL734CreateContollerCallFunc);
} }
extern "C" { extern "C"
epicsExportRegistrar(EL734Register); {
epicsExportRegistrar(EL734Register);
} }

25
sinqEPICSApp/src/EL734Driver.h
View File

@ -28,31 +28,32 @@ public:
asynStatus setClosedLoop(bool closedLoop); asynStatus setClosedLoop(bool closedLoop);
private: private:
EL734Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. EL734Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
* Abbreviated because it is used very frequently */ * Abbreviated because it is used very frequently */
int oredMSR; int oredMSR;
double position; double position;
int homing; int homing;
time_t next_poll; time_t next_poll;
int errorReported; int errorReported;
friend class EL734Controller; friend class EL734Controller;
}; };
class EL734Controller : public SINQController { class EL734Controller : public SINQController
{
public: public:
EL734Controller(const char *portName, const char *EL734PortName, int numAxes); EL734Controller(const char *portName, const char *EL734PortName, int numAxes);
void report(FILE *fp, int level); void report(FILE *fp, int level);
EL734Axis* getAxis(asynUser *pasynUser); EL734Axis *getAxis(asynUser *pasynUser);
EL734Axis* getAxis(int axisNo); EL734Axis *getAxis(int axisNo);
friend class EL734Axis; friend class EL734Axis;
private:
asynUser *pasynUserController_;
asynStatus transactController(int axis, char command[COMLEN], char reply[COMLEN]); private:
asynUser *pasynUserController_;
void switchRemote(); asynStatus transactController(int axis, char command[COMLEN], char reply[COMLEN]);
void switchRemote();
}; };