lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity
Files
83af8f1d92aae6a057ceea3a3515fa3c2d67cfd6
motorBase/motorApp/OmsAsynSrc/omsBaseController.cpp
T

1100 lines
38 KiB
C++
Raw Blame History

/*
FILENAME... omsBaseController.cpp
USAGE... Pro-Dex OMS asyn motor base controller support
Version: $Revision$
Modified By: $Author$
Last Modified: $Date$
HeadURL: $URL$
*/
/*
* Created on: 06/2012
* Author: eden
*
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <epicsExit.h>
#include "omsBaseController.h"
#define MIN(a,b) ((a)<(b)? (a): (b))
#define motorOmsStringSendString "OMS_STRING_SEND"
#define motorOmsStringSendRecvString "OMS_STRING_SENDRECV"
#define motorOmsStringRecvString "OMS_STRING_RECV"
#define motorOmsPollString "OMS_POLL"
#define MOTOR_OMS_PARAMS_COUNT 4
ELLLIST omsBaseController::omsControllerList;
int omsBaseController::omsTotalControllerNumber = 0;
static const char *driverName = "omsBaseDriver";
#ifdef __GNUG__
#ifdef DEBUG
#define Debug(l, f, args...) {if (l <= motorOMSBASEdebug) \
errlogPrintf(f, ## args);}
#else
#define Debug(l, f, args...)
#endif
#else
#define Debug
#endif
volatile int motorOMSBASEdebug = 0;
extern "C" {epicsExportAddress(int, motorOMSBASEdebug);}
omsBaseController::omsBaseController(const char *portName, int maxAxes, int prio, int stackSz, int extMotorParams=0)
: asynMotorController(portName, maxAxes, extMotorParams + MOTOR_OMS_PARAMS_COUNT,
asynInt32Mask | asynFloat64Mask | asynOctetMask,
asynInt32Mask | asynFloat64Mask | asynOctetMask,
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
1, // autoconnect
prio, stackSz)
{
if (omsTotalControllerNumber == 0) {
ellInit(&omsControllerList);
}
// check if portName is in use */
if (findController(portName) != NULL)
errlogPrintf("omsBaseController: ERROR: asynPort %s already in use\n", portName);
omsBaseNode* pNode = new omsBaseNode;
pNode->portName = epicsStrDup(portName);
pNode->pController = this;
ellAdd(&omsControllerList, (ELLNODE *)pNode);
this->portName = epicsStrDup(portName);
controllerNumber = omsTotalControllerNumber;
++omsTotalControllerNumber;
sanityCounter = 0;
useWatchdog = false;
enabled = true;
numAxes = maxAxes;
controllerType = NULL;
baseMutex = new epicsMutex;
if (prio == 0)
priority = epicsThreadPriorityLow;
else
priority = prio;
if (stackSz == 0)
stackSize = epicsThreadGetStackSize(epicsThreadStackMedium);
else
stackSize = stackSz;
createParam(0, motorOmsPollString, asynParamInt32, &pollIndex);
createParam(0, motorOmsStringSendString, asynParamOctet, &sendIndex);
createParam(0, motorOmsStringSendRecvString, asynParamOctet, &sendReceiveIndex);
createParam(0, motorOmsStringRecvString, asynParamOctet, &receiveIndex);
setStringParam(0, sendIndex, (char *) "");
setStringParam(0, sendReceiveIndex, (char *) "");
setStringParam(0, receiveIndex, (char *) "");
/* Set an EPICS exit handler */
epicsAtExit(omsBaseController::callShutdown, this);
}
asynStatus omsBaseController::startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls)
{
char threadName[20];
movingPollPeriod_ = movingPollPeriod/1000.;
idlePollPeriod_ = idlePollPeriod/1000.;
forcedFastPolls_ = forcedFastPolls;
epicsSnprintf(threadName, sizeof(threadName), "OMSPoller-%d", controllerNumber);
this->motorThread = epicsThreadCreate(threadName,
priority, stackSize,
(EPICSTHREADFUNC) &omsBaseController::callPoller, (void *) this);
return asynSuccess;
}
void omsBaseController::shutdown(){
lock();
shuttingDown_ = 1;
unlock();
}
void omsBaseController::report(FILE *fp, int level)
{
int axis;
double velocity, position, encoderPosition;
int haveEncoder=0;
fprintf(fp, "Oms %s motor driver %s, numAxes=%d\n",
controllerType, portName, numAxes);
for (axis=0; axis < numAxes; axis++) {
omsBaseAxis *pAxis = pAxes[axis];
fprintf(fp, " axis %d\n", pAxis->axisNo_);
if (level > 0)
{
lock();
getDoubleParam(pAxis->axisNo_, motorVelocity_, &velocity);
getDoubleParam(pAxis->axisNo_, motorPosition_, &position);
getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
if (haveEncoder) getDoubleParam(pAxis->axisNo_, motorEncoderPosition_, &encoderPosition);
unlock();
fprintf(fp, " Current position = %f, velocity = %f\n",
position,
velocity);
if (haveEncoder) fprintf(fp, " Encoder position %f\n", encoderPosition );
if (pAxis->homing) fprintf(fp, " Currently homing axis\n" );
}
}
// Call the base class method
asynMotorController::report(fp, level);
}
omsBaseAxis * omsBaseController::getAxis(asynUser *pasynUser)
{
int axisNo;
getAddress(pasynUser, &axisNo);
return pAxes[axisNo];
}
/** Returns a pointer to an omsBaseAxis object.
* Returns NULL if the axis number is invalid.
* \param[in] axisNo Axis index number. */
omsBaseAxis* omsBaseController::getAxis(int axisNo)
{
if ((axisNo < 0) || (axisNo >= numAxes)) return NULL;
return pAxes[axisNo];
}
asynStatus omsBaseController::writeOctet(asynUser *pasynUser, const char *value,
size_t nChars, size_t *nActual)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
const char *functionName = "writeOctet";
char inBuffer[MAX_STRING_SIZE];
if (strlen(value) > nChars) return asynError;
omsBaseAxis *pAxis = getAxis(pasynUser);
if (!pAxis) return asynError;
if (function == sendReceiveIndex)
{
status = sendReceiveLock(value, inBuffer, sizeof(inBuffer));
if (status == asynSuccess){
/* Set the parameter in the parameter library. */
status = (asynStatus)setStringParam(pAxis->axisNo_, receiveIndex, (char *)inBuffer);
status = (asynStatus)callParamCallbacks(pAxis->axisNo_);
asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: answer is %s\n",
driverName, functionName, portName, inBuffer);
*nActual = nChars;
}
else {
*nActual = 0;
asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: sendReceive Error\n",
driverName, functionName, portName);
}
}
else if (function == sendIndex)
{
status = sendOnlyLock(value);
if (status == asynSuccess){
*nActual = nChars;
}
else {
*nActual = 0;
asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: send Error\n",
driverName, functionName, portName);
}
}
return status;
}
asynStatus omsBaseController::readInt32(asynUser *pasynUser, epicsInt32 *value)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
omsBaseAxis *pAxis = getAxis(pasynUser);
static const char *functionName = "readInt32";
static char outputBuffer[8];
if (!pAxis) return asynError;
if (function == motorPosition_) {
strcpy(outputBuffer,"A? RP");
sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
*value = strtol(inputBuffer, NULL, 10);
} else if (function == motorEncoderPosition_) {
int haveEncoder;
getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
if (haveEncoder){
strcpy(outputBuffer,"A? RE");
sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
*value = strtol(inputBuffer, NULL, 10);
}
} else {
// Call base class
status = asynMotorController::readInt32(pasynUser, value);
}
return status;
}
asynStatus omsBaseController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
static const char *functionName = "writeInt32";
omsBaseAxis *pAxis = getAxis(pasynUser);
if (!pAxis) return asynError;
status = pAxis->setIntegerParam(function, value);
if (function == motorDeferMoves_)
{
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s:%s Deferred Move: not yet implemented %s\n",
driverName, functionName, portName);
}
else if (function == motorClosedLoop_)
{
if (value) {
asynPrint(pasynUser, ASYN_TRACE_FLOW, "%s:%s:%s axis %d closed loop enable\n",
driverName, functionName, portName, pAxis->axisNo_);
if (firmwareMin(1,30,0))
status = sendReplace(pAxis, (char*) "A? CL1");
else
status = sendReplace(pAxis, (char*) "A? HN");
} else {
asynPrint(pasynUser, ASYN_TRACE_FLOW, "%s:%s:%s SetInteger axis %d closed loop disable\n",
driverName, functionName, portName, pAxis->axisNo_);
if (firmwareMin(1,30,0))
status = sendReplace(pAxis, (char*) "A? CL0");
else
status = sendReplace(pAxis, (char*) "A? HF");
}
}
else if (function == motorMoveToHome_) {
/* avoid asynMotorController::writeInt32 to handle this*/
}
else if (function == pollIndex)
{
if (value) {
wakeupPoller();
}
}
else {
return asynMotorController::writeInt32(pasynUser, value);
}
pAxis->callParamCallbacks();
if (status)
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s: error, status=%d function=%d, value=%d\n",
driverName, functionName, status, function, value);
else
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
"%s:%s: function=%d, value=%d\n",
driverName, functionName, function, value);
return status;
}
asynStatus omsBaseController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
static const char *functionName = "writeFloat64";
omsBaseAxis *pAxis = getAxis(pasynUser);
if (!pAxis) return asynError;
/* Set the parameter and readback in the parameter library. This may be overwritten when we read back the
* status at the end, but that's OK */
status = pAxis->setDoubleParam(function, value);
if (function == motorEncoderPosition_){
int haveEncoder;
getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
if (haveEncoder){
asynPrint(pasynUser, ASYN_TRACE_FLOW,
"%s:%s:%s axis %d set encoder position to %f\n",
driverName, functionName, portName, pAxis->axisNo_, value);
sprintf(inputBuffer,"A? LPE%d;",(int)(value));
status = sendReplace(pAxis, inputBuffer);
}
}
else if (function == motorEncoderRatio_) {
int haveEncoder;
getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
if (haveEncoder){
asynPrint(pasynUser, ASYN_TRACE_FLOW,
"%s:%s:%s axis %d set encoder ratio to %f\n",
driverName, functionName, portName, pAxis->axisNo_, value);
sprintf(inputBuffer,"A? ER%d,100000;",(int)(value*100000));
status = sendReplace(pAxis, inputBuffer);
}
}
else if ((function == motorResolution_) || (function == motorLowLimit_) || (function == motorHighLimit_)) {
// we do nothing
status = asynSuccess;
}
else if (function == motorPGain_) {
if ((0.00 >= value) && (value < 32768.00)){
asynPrint(pasynUser, ASYN_TRACE_FLOW,
"%s:%s:%s axis %d set proportional gain to %f\n",
driverName, functionName, portName, pAxis->axisNo_, value);
sprintf(inputBuffer,"A? KP%f;", value);
status = sendReplace(pAxis, inputBuffer);
}
else {
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s:%s axis %d proportional gain %f not in range 0.0 -> 32768.0\n",
driverName, functionName, portName, pAxis->axisNo_, value);
}
}
else if (function == motorIGain_) {
if ((0.00 >= value) && (value < 32768.00)){
asynPrint(pasynUser, ASYN_TRACE_FLOW,
"%s:%s:%s axis %d set integral gain to %f\n",
driverName, functionName, portName, pAxis->axisNo_, value);
sprintf(inputBuffer,"A? KI%f;", value);
status = sendReplace(pAxis, inputBuffer);
}
else {
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s:%s axis %d integral gain %f not in range 0.0 -> 32768.0\n",
driverName, functionName, portName, pAxis->axisNo_, value);
}
}
else if (function == motorDGain_)
{
if ((0.00 >= value) && (value < 32768.00)){
asynPrint(pasynUser, ASYN_TRACE_FLOW,
"%s:%s:%s axis %d set derivative gain to %f\n",
driverName, functionName, portName, pAxis->axisNo_, value);
sprintf(inputBuffer,"A? KD%f;", value);
status = sendReplace(pAxis, inputBuffer);
}
else {
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s:%s axis %d derivative gain %f not in range 0.0 -> 32768.0\n",
driverName, functionName, portName, pAxis->axisNo_, value);
}
}
else {
/* Call base classes method (if we have our parameters check this here) */
status = asynMotorController::writeFloat64(pasynUser, value);
}
pAxis->callParamCallbacks();
if (status)
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s: error, status=%d function=%d, value=%f\n",
driverName, functionName, status, function, value);
else
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
"%s:%s: function=%d, value=%f\n",
driverName, functionName, function, value);
return status;
}
asynStatus omsBaseController::getFirmwareVersion()
{
/* get FirmwareVersion, try 3 times */
int count = 0;
asynStatus status = asynError;
char *p;
while (status != asynSuccess && count < 3){
epicsThreadSleep(1.0);
status = sendReceiveLock((char*) "WY", inputBuffer, sizeof(inputBuffer));
count++;
errlogPrintf("OMS Firmware Version: %s\n", inputBuffer);
}
//
if ((p = strstr(inputBuffer, "ver:"))){
count = sscanf(p, "ver:%d.%d.%d,", &fwMajor, &fwMinor, &fwRevision);
}
if ((p == NULL) || (count < 2)) {
errlogPrintf("omsBaseController::getFirmwareVersion: unable to retrieve Firmware version\n");
status = asynError;
}
return status;
}
asynStatus omsBaseController::Init(const char* initString, int multiple){
char *p, *tokSave;
int totalAxes;
char axisChrArr[OMS_MAX_AXES] = {'X','Y','Z','T','U','V','R','S'};
char outputBuffer[10];
/* Interrupt clear */
sendOnlyLock("IC;");
/* Stop all axes */
sendOnlyLock("AM SA;");
/* send InitString */
if ((initString != NULL) && (strlen(initString) > 0)) {
if (multiple){
char* inittmp = epicsStrDup(initString);
for (tokSave = NULL, p = epicsStrtok_r(inittmp, ";", &tokSave);
p != NULL; p = epicsStrtok_r(NULL, ";", &tokSave)) {
sendOnlyLock(p);
}
free(inittmp);
}
else {
sendOnlyLock(initString);
}
}
/* get Positions of all axes */
inputBuffer[0] = '\0';
sendReceiveLock((char*) "AA RP;", inputBuffer, sizeof(inputBuffer));
if (numAxes > OMS_MAX_AXES) {
errlogPrintf("omsBaseController:Init: number of axes=%d exceeds allowed maximum\n", numAxes);
return asynError;
}
/* The return string will tell us how many axes this controller has */
for (totalAxes = 0, tokSave = NULL, p = epicsStrtok_r(inputBuffer, ",", &tokSave);
p != 0; p = epicsStrtok_r(NULL, ",", &tokSave), totalAxes++);
if ((totalAxes > numAxes) || (totalAxes > OMS_MAX_AXES)) {
errlogPrintf("omsBaseController:Init: actual number of axes=%d > MIN(numAxes=%d, OMS_MAX_AXES)\n", totalAxes, numAxes);
return asynError;
}
numAxes = totalAxes;
pAxes = new omsBaseAxis*[numAxes];
lock();
for (int axis=0; axis < numAxes; axis++) {
omsBaseAxis* pAxis = new omsBaseAxis(this, axis, axisChrArr[axis]);
pAxes[axis] = pAxis;
pAxis->setIntegerParam(motorStatusDone_, 1);
pAxis->setIntegerParam(motorStatusMoving_, 0);
pAxis->setIntegerParam(motorStatusHomed_, 0);
pAxis->setIntegerParam(motorStatusHome_, 0);
pAxis->setIntegerParam(motorStatusAtHome_, 0);
pAxis->setIntegerParam(motorStatusProblem_, 0);
pAxis->setIntegerParam(motorStatusCommsError_, 0);
/* Determine if encoder is present and if mode is stepper or servo. */
if (firmwareMin(1,1,30))
strcpy(outputBuffer,"A? PS?");
else
strcpy(outputBuffer,"A? ?PS");
sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
/*we expect any of "=O","=E","=M" */
if (inputBuffer[1] == 'O'){
pAxis->setStepper(1);
pAxis->setIntegerParam(motorStatusHasEncoder_, 0);
pAxis->setDoubleParam(motorEncoderPosition_, 0.0); // reset if not available
}
else if (inputBuffer[1] == 'M'){
pAxis->setStepper(0);
pAxis->setIntegerParam(motorStatusHasEncoder_, 1);
}
else if (inputBuffer[1] == 'E'){
pAxis->setStepper(1);
pAxis->setIntegerParam(motorStatusHasEncoder_, 1);
}
else
errlogPrintf("omsBaseController:Init: error: unknown axis type! (%s)\n", inputBuffer);
/* Determine limit true state high or low */
/* CAUTION you need firmware version 1.30 or higher to do this */
if (firmwareMin(1,1,30))
strcpy(outputBuffer,"A? LT?");
else
strcpy(outputBuffer,"A? ?LS");
sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
/*we expect any of "=l" or "=h" */
if (inputBuffer[1] == 'l'){
pAxis->setLimitInvert(1);
}
else if (inputBuffer[1] == 'h'){
pAxis->setLimitInvert(0);
}
else
errlogPrintf("omsBaseController:Init: error: unknown limit true state!\n");
}
unlock();
return asynSuccess;
}
void omsBaseController::callPoller(void *drvPvt)
{
omsBaseController *pController = (omsBaseController*)drvPvt;
pController->omsPoller();
}
#define DELTA 0.1
void omsBaseController::omsPoller()
{
static const char *functionName = "BasePoller";
#define STATUSSTRINGLEN 5
/* This is the task that polls the MAX card */
double timeout, timeToWait;
double movingPollPeriod;
double idlePollPeriod;
omsBaseAxis* pAxis;
int retry_count, loopBreakCount=0, haveEncoder;
// asynStatus status;
int axisMoving, anyMoving=0;
int forcedFastPolls, fastPolls=0;
epicsInt32 axisPosArr[OMS_MAX_AXES], encPosArr[OMS_MAX_AXES], veloArr[OMS_MAX_AXES];
char statusBuffer[OMS_MAX_AXES*STATUSSTRINGLEN+2];
char encStatusBuffer[OMS_MAX_AXES*6+2];
int closedLoopStatus[OMS_MAX_AXES];
char encBuffer[7];
unsigned int limitFlags;
epicsTimeStamp now, loopStart;
bool haveCLStatus, haveVeloArray, haveEncStatus, haveLimits, useEncoder=false, moveDone;
lock();
movingPollPeriod = movingPollPeriod_;
idlePollPeriod = idlePollPeriod_;
forcedFastPolls = forcedFastPolls_;
for (int i=0; (i < numAxes); i++) {
getIntegerParam(i, motorStatusHasEncoder_, &haveEncoder);
if (haveEncoder) useEncoder = true;
}
unlock();
while(1) {
lock();
if (shuttingDown_) {
unlock();
break;
}
unlock();
if (loopBreakCount > 10){
errlogPrintf("%s:%s:%s: Error: %d consecutive unsuccessful attempts to read from motor card\n"
, driverName, functionName, this->portName, loopBreakCount);
loopBreakCount = 0;
}
epicsTimeGetCurrent(&loopStart);
retry_count = 0;
while ((getAxesPositions(axisPosArr) != asynSuccess) && (retry_count < 5)){
epicsThreadSleepQuantum();
++retry_count;
}
if (retry_count > 4){
errlogPrintf("%s:%s:%s: error reading axis position after %d attempts\n",
driverName, functionName, this->portName, retry_count);
++loopBreakCount;
continue;
}
if (useEncoder && (getEncoderPositions(encPosArr) != asynSuccess)){
Debug(1, "%s:%s:%s: error executing get Encoder Positions\n", driverName, functionName, this->portName);
++loopBreakCount;
continue;
}
/* read all axis status values and reset done-field
* MDNN,MDNN,PNLN,PNNN,PNLN,PNNN,PNNN,PNNN*/
if (getAxesStatus(statusBuffer, sizeof(statusBuffer), &moveDone) != asynSuccess){
Debug(1, "%s:%s:%s: error reading axes status\n", driverName, functionName, this->portName);
++loopBreakCount;
continue;
}
loopBreakCount = 0;
/*
if (sanityCheck() != asynSuccess){
errlogPrintf("%s:%s:%s: error during sanity check\n", driverName, functionName, this->portName);
}
*/
if (anyMoving)
haveCLStatus = false;
else
haveCLStatus = true;
if (haveCLStatus && getClosedLoopStatus(closedLoopStatus) != asynSuccess){
haveCLStatus = false;
Debug(1, "%s:%s:%s: error executing get Closed Loop Status\n", driverName, functionName, this->portName);
}
haveVeloArray = true;
if (getAxesArray((char*) "AM;RV;", veloArr) != asynSuccess){
haveVeloArray = false;
Debug(1,"%s:%s:%s: Error executing command Report Velocity (RV)\n", driverName, functionName, this->portName);
}
haveEncStatus = true;
if (sendReceiveLock((char*) "AM;EA;", encStatusBuffer, sizeof(encStatusBuffer)) != asynSuccess){
haveEncStatus = false;
Debug(1,"%s:%s:%s: Error reading encoder status buffer >%s<\n", driverName, functionName, this->portName, encStatusBuffer);
}
/* read all limits */
if (moveDone || (anyMoving == 0))
haveLimits = true;
else
haveLimits = false;
limitFlags =0;
if (haveLimits){
if ((sendReceiveLock((char*) "AM;QL;", pollInputBuffer, sizeof(pollInputBuffer)) == asynSuccess)){
if (1 != sscanf(pollInputBuffer, "%x", &limitFlags)){
Debug(1,"%s:%s:%s: error converting limits: %s\n", driverName, functionName, this->portName, pollInputBuffer);
haveLimits = false;
}
}
else {
haveLimits = false;
Debug(1,"%s:%s:%s: error reading limits %s\n", driverName, functionName, this->portName, pollInputBuffer);
}
}
if (enabled) watchdogOK();
anyMoving = 0;
lock();
for (int i=0; (i < numAxes) && enabled && (shuttingDown_ == 0); i++) {
pAxis = pAxes[i];
if (useEncoder){
/* if this axis has an encoder, read encoder status */
getIntegerParam(i, motorStatusHasEncoder_, &haveEncoder);
if (haveEncoder){
if (haveEncStatus){
if (getSubstring(i, encStatusBuffer, encBuffer, sizeof(encBuffer)) == asynSuccess){
if (encBuffer[2] == 'S')
pAxis->setIntegerParam(motorStatusFollowingError_, 1);
else
pAxis->setIntegerParam(motorStatusFollowingError_, 0);
}
else{
errlogPrintf("%s:%s:%s: error parsing encoder status string %s\n",
driverName, functionName, this->portName, encStatusBuffer);
}
}
pAxis->setDoubleParam(motorEncoderPosition_, (double) encPosArr[i]);
}
}
/* check the done flag or current velocity */
if (statusBuffer[i*STATUSSTRINGLEN + 1] == 'D'){
Debug(3, "%s:%s:%s: found Done Flag axis %d\n", driverName, functionName, portName, i);
pAxis->setIntegerParam(motorStatusProblem_, 0);
pAxis->moveDelay=0;
pAxis->setIntegerParam(motorStatusDone_, 1);
pAxis->setIntegerParam(motorStatusMoving_, 0);
if (pAxis->homing) pAxis->homing = 0;
Debug(1, "%s:%s:%s: done axis %d \n", driverName, functionName,
this->portName, i );
}
else if (haveVeloArray && (veloArr[i] == 0)){
/* keep a small delay here, to make sure that a motorAxisDone cycle is performed
even if the motor is on hard limits */
getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving);
if (axisMoving){
pAxis->moveDelay++ ;
if (pAxis->moveDelay >= 5) {
Debug(4, "%s:%s:%s: setting Problem Flag axis %d\n", driverName, functionName, portName, i);
pAxis->setIntegerParam(motorStatusDone_, 1);
pAxis->setIntegerParam(motorStatusMoving_, 0);
pAxis->setIntegerParam(motorStatusProblem_, 1);
pAxis->moveDelay = 0;
if (pAxis->homing) pAxis->homing = 0;
Debug(1, "%s:%s:%s: stop axis %d, moveDelay count %d\n", driverName, functionName,
this->portName, i, pAxis->moveDelay );
}
Debug(2, "%s:%s:%s: moveDelay axis %d, count %d\n", driverName, functionName,
this->portName, i, pAxis->moveDelay );
}
}
else {
Debug(4, "%s:%s:%s: poller loop: axis %d still moving\n", driverName, functionName, this->portName, i);
pAxis->moveDelay = 0;
pAxis->setIntegerParam(motorStatusProblem_, 0);
pAxis->setIntegerParam(motorStatusDone_, 0);
pAxis->setIntegerParam(motorStatusMoving_, 1);
}
getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving);
anyMoving += axisMoving;
/* check limits */
if (haveLimits){
if (((limitFlags & (1 << i)) > 0) ^ (pAxis->getLimitInvert()))
pAxis->setIntegerParam(motorStatusLowLimit_, 1);
else
pAxis->setIntegerParam(motorStatusLowLimit_, 0);
if (((limitFlags & (1 << (i+8))) > 0) ^ (pAxis->getLimitInvert()))
pAxis->setIntegerParam(motorStatusHighLimit_, 1);
else
pAxis->setIntegerParam(motorStatusHighLimit_, 0);
}
/* check home switch */
if (statusBuffer[i*STATUSSTRINGLEN + 3] == 'H')
pAxis->setIntegerParam(motorStatusAtHome_, 1);
else
pAxis->setIntegerParam(motorStatusAtHome_, 0);
/* check direction */
if (statusBuffer[i*STATUSSTRINGLEN] == 'P')
pAxis->setIntegerParam(motorStatusDirection_, 1);
else
pAxis->setIntegerParam(motorStatusDirection_, 0);
/* set positions */
pAxis->setDoubleParam(motorPosition_, (double) axisPosArr[i]);
/* set closed loop status */
if (haveCLStatus) pAxis->setIntegerParam(motorStatusGainSupport_, closedLoopStatus[i]);
// callParamCallbacks(pAxis->axisNo_, pAxis->axisNo_);
pAxis->callParamCallbacks();
} /* Next axis */
if (shuttingDown_) {
unlock();
break;
}
unlock();
if (fastPolls > 0) {
timeout = movingPollPeriod;
fastPolls--;
}
else if (anyMoving) {
timeout = movingPollPeriod;
}
else {
timeout = idlePollPeriod;
}
/* wait here for the next poll
waiting may be interrupted by pollEvent or interrupt messages*/
epicsTimeGetCurrent(&now);
timeToWait = timeout - epicsTimeDiffInSeconds(&now, &loopStart);
Debug(5, "%s:%s:%s: poller loop: waiting %f s\n", driverName, functionName, this->portName, timeToWait);
if (waitInterruptible(timeToWait) == epicsEventWaitOK) {
fastPolls = forcedFastPolls;
}
} /* End while */
Debug(1, "%s:%s:%s: omsPoller shutdown\n", driverName, functionName, portName);
}
asynStatus omsBaseController::getEncoderPositions(epicsInt32 encPosArr[OMS_MAX_AXES])
{
return getAxesArray((char*) "AM PE;", encPosArr);
}
asynStatus omsBaseController::getClosedLoopStatus(int clstatus[OMS_MAX_AXES])
{
asynStatus status = asynSuccess;
char clBuffer[9];
if (firmwareMin(1,30,0)){
pollInputBuffer[0] = '\0';
status = sendReceiveLock((char*) "AM;CL?;", pollInputBuffer, sizeof(pollInputBuffer));
if (status == asynSuccess) {
for (int i=0; i < numAxes; ++i) {
status = getSubstring(i, pollInputBuffer, clBuffer, sizeof(clBuffer));
if ( status == asynSuccess){
if (strncmp(clBuffer, "on", 2))
clstatus[i] = 1;
else
clstatus[i] = 0;
}
}
}
}
else {
for (int i=0; i < numAxes; ++i) {
strcpy(clBuffer,"A? ?PM");
status = sendReceiveReplace(pAxes[i], clBuffer, pollInputBuffer, sizeof(pollInputBuffer));
if (status != asynSuccess){
Debug(1, "%s:getClosedLoopStatus:%s: Error getting closed loop status %s\n",
driverName, portName, pollInputBuffer);
}
else {
if (strncmp(pollInputBuffer, "=on", 3))
clstatus[i] = 1;
else
clstatus[i] = 0;
}
}
}
return status;
}
asynStatus omsBaseController::sanityCheck()
{
const char* functionName="sanityCheck";
asynStatus status = asynSuccess;
++ sanityCounter;
if (sanityCounter > 100){
int commandBufferSize[OMS_MAX_AXES];
char outputBuffer[10];
// check the remaining command buffer size, which must not overflow
sanityCounter = 0;
if (getAxesArray((char*) "AM;RQC", commandBufferSize) != asynSuccess){
errlogPrintf("%s:%s:%s: Error executing command: Report Command Queue (RCQ)\n", driverName, functionName, this->portName);
}
for (int i=0; i < numAxes; i++) {
omsBaseAxis* pAxis = pAxes[i];
// flush command buffer if size is below 100
if (commandBufferSize[i] < 100) {
errlogPrintf("%s:%s:%s: Caution: flushing command queue axis %d,remaining size %d\n",
driverName, functionName, portName, pAxis->axisNo_, commandBufferSize[i]);
strcpy(outputBuffer,"A?;FL;");
sendReplace(pAxis, (char*) outputBuffer);
status = asynError;
}
}
}
return status;
}
bool omsBaseController::firmwareMin(int major, int minor, int revision){
if (major < fwMajor) return true;
if (major == fwMajor){
if (minor < fwMinor) return true;
if (minor == fwMinor){
if (revision <= fwRevision) return true;
}
}
return false;
}
omsBaseController* omsBaseController::findController(const char* asynPort){
omsBaseNode* pNode = (omsBaseNode*) ellFirst(&omsControllerList);
while (pNode) {
if (strcmp(pNode->portName, asynPort) == 0){
return (pNode->pController);
}
pNode = (omsBaseNode *)ellNext(&pNode->node);
}
return (omsBaseController *) NULL;
}
epicsEventWaitStatus omsBaseController::waitInterruptible(double timeout)
{
return epicsEventWaitWithTimeout(pollEventId_, timeout);
}
/*
insert axis character and send to Controller
If insertChar != 0 and 2nd Character is a "?", then replace it with axisChar
*/
asynStatus omsBaseController::sendReplace(omsBaseAxis* pAxis, char *outputBuff)
{
asynStatus status;
if (strncmp(outputBuff,"A? ",3) == 0 )
outputBuff[1] = pAxis->axisChar;
status = sendOnlyLock(outputBuff);
return status;
}
/*
insert axis character, send to Controller and wait for the answer
If insertChar != 0 and 2nd Character is a "?", then replace it with axisChar
*/
asynStatus omsBaseController::sendReceiveReplace(omsBaseAxis* pAxis, char *outputBuff, char *inputBuff, int inputSize)
{
asynStatus status;
if (strncmp(outputBuff,"A? ",3) == 0 )
outputBuff[1] = pAxis->axisChar;
status = sendReceiveLock(outputBuff, inputBuff, inputSize);
return status;
}
asynStatus omsBaseController::sendOnlyLock(const char *outputBuff)
{
asynStatus status;
baseMutex->lock();
status = sendOnly(outputBuff);
baseMutex->unlock();
return status;
}
asynStatus omsBaseController::sendReceiveLock(const char *outputBuff, char *inputBuff, unsigned int inputSize)
{
asynStatus status;
baseMutex->lock();
status = sendReceive(outputBuff, inputBuff, inputSize);
baseMutex->unlock();
return status;
}
asynStatus omsBaseController::getAxesStatus(char *inputBuff, int inputSize, bool *done)
{
char *outputBuff = (char*) "AM;RI;";
asynStatus status;
*done=false;
status = sendReceiveLock(outputBuff, inputBuff, inputSize);
if (status == asynSuccess){
if (strchr(inputBuff, 'D') != NULL) *done=true;
if (!((inputBuff[0] == 'P') || (inputBuff[0] == 'M')))
status = asynError;
if (strlen(inputBuff) < (unsigned int)(numAxes * 5 -1))
status = asynError;
if (status == asynError) Debug(1, "%s:getAxesStatus:%s: corrupted status string %s\n",
driverName, portName, inputBuff);
}
return status;
}
asynStatus omsBaseController::getAxesPositions(int positions[OMS_MAX_AXES] )
{
return getAxesArray((char*) "AM PP;", positions);
}
asynStatus omsBaseController::getAxesArray(char* cmd, int positions[OMS_MAX_AXES] )
{
// maximum length of the ascii position / velocity values is 11 chars + comma
// -2147483648 <-> 2147483647
// we expect numAxes values separated with commas
// possible answers are "0,5000,0" ",,,," "0" ",,," (3 commas for 4 axes)
const char* functionName="getAxesArray";
asynStatus status = asynSuccess;
char inputBuff[OMSINPUTBUFFERLEN] = "";
char *start, *end, *stop;
int i, intVal, again = 1;
int count =0;
status = sendReceiveLock(cmd, inputBuff, sizeof(inputBuff));
if ((status == asynSuccess) && (strlen(inputBuff) >= (unsigned int)numAxes -1)) {
start = inputBuff;
stop = start + MIN(strlen(inputBuff), sizeof(inputBuff));
for (i = 0; ((i < OMS_MAX_AXES) && again); ++i){
if (*start == ','){
positions[i] = 0;
++count;
start += 1;
if (*start == '\0'){
/* comma was last character */
++count;
again=0;
if (i < OMS_MAX_AXES-1) positions[i+1] = 0;
}
continue;
}
intVal = strtol(start, &end, 10);
if ((intVal == 0) && (start == end)) {
again = 0;
}
else {
positions[i] = intVal;
++count;
}
if (end >= stop) again = 0;
start = end + 1;
}
if (count != numAxes) {
errlogPrintf("%s:%s:%s: array string conversion error, count: %d, axes: %d, input: >%s<\n",
driverName, functionName, portName, count, numAxes, inputBuff);
return asynError;
}
}
else {
if (status == asynSuccess){
errlogPrintf("%s:%s:%s: read string too short %d\n",
driverName, functionName, portName, (int)strlen(inputBuff));
return asynError;
}
}
return status;
}
asynStatus omsBaseController::getSubstring(unsigned int number, char* inputBuffer, char *outBuffer, unsigned int outBufferLen)
{
/* get the numbered substring from a comma-separated string */
/* numbering starts with 0 */
/* other as strtok accept a sequence of commas ,,, as empty substrings */
char *start, *end, *pos, *tmpBuffer;
unsigned int i, count;
int doloop = 1;
asynStatus status = asynError;
if (strlen(inputBuffer) < number) return status;
tmpBuffer = epicsStrDup(inputBuffer);
start = tmpBuffer;
end = start + strlen(start);
for (i=0; i<=number && doloop==1; ++i){
pos = strchr(start, ',');
if (pos == NULL) {
doloop = 0;
count = MIN(strlen(start), outBufferLen-1);
}
else {
count = MIN((unsigned int)(pos-start), outBufferLen-1);
*pos = '\0';
}
if (i==number){
strncpy(outBuffer, start, count);
outBuffer[count]='\0';
status = asynSuccess;
break;
}
start = pos+1;
if (start > end) doloop =0;
}
free (tmpBuffer);
return status;
}
bool omsBaseController::watchdogOK()
{
char inputBuff[10] = "";
const char* functionName = "watchdogOK";
if (useWatchdog && (fwMinor >= 33)) {
sendReceiveLock((char*) "#WS", (char*) inputBuff, sizeof(inputBuff));
if ((inputBuff[0] == '=') && (inputBuff[1] != '0')) {
errlogPrintf("%s:%s:%s: *** CAUTION watchdog not running, disabling card ***\n",
driverName, functionName, portName);
enabled = false;
return false;
}
}
return true;
}
View Git Blame Copy Permalink