889 lines
28 KiB
C++
889 lines
28 KiB
C++
/*
|
|
Driver for the MasterMACS motor controller used at SINQ
|
|
For documentation see the standard SINQ place for hardware documentation or
|
|
Marcel Schildt
|
|
|
|
Mark Koennecke, March-August 2023
|
|
*/
|
|
|
|
|
|
#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 "MasterMACSDriver.h"
|
|
#include <epicsExport.h>
|
|
|
|
#define CHECK_BIT(var,pos) ((var) & (1 << pos))
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
|
|
#define debug 0
|
|
|
|
/** Creates a new MasterMACSController object.
|
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
|
* \param[in] MasterMACSPortName The name of the drvAsynSerialPort that was created previously to connect to the MasterMACS controller
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
*/
|
|
MasterMACSController::MasterMACSController(const char *portName,
|
|
const char *MasterMACSPortName,
|
|
int numAxes,
|
|
int idlePoll, int busyPoll):SINQController
|
|
(portName, MasterMACSPortName, numAxes)
|
|
{
|
|
asynStatus status;
|
|
static const char *functionName =
|
|
"MasterMACSController::MasterMACSController";
|
|
char terminator[2] = "\x03";
|
|
|
|
|
|
/* Connect to MasterMACS controller */
|
|
status =
|
|
pasynOctetSyncIO->connect(MasterMACSPortName, 0,
|
|
&pasynUserController_, NULL);
|
|
if (status) {
|
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
|
"%s: cannot connect to MasterMACS controller\n",
|
|
functionName);
|
|
}
|
|
pasynOctetSyncIO->setOutputEos(pasynUserController_, terminator,
|
|
strlen(terminator));
|
|
pasynOctetSyncIO->setInputEos(pasynUserController_, terminator,
|
|
strlen(terminator));
|
|
|
|
|
|
pAxes_ = (MasterMACSAxis **) (asynMotorController::pAxes_);
|
|
createParam(EnableAxisString, asynParamInt32, &enableAxis_);
|
|
createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
|
|
callParamCallbacks();
|
|
|
|
startPoller(busyPoll/1000., idlePoll/1000., 1);
|
|
setIdlePollPeriod(idlePoll/1000.);
|
|
setMovingPollPeriod(busyPoll/1000.);
|
|
}
|
|
|
|
|
|
/** Creates a new MasterMACSController 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] MasterMACSPortName The name of the drvAsynIPPPort that was created previously to connect to the MasterMACS controller
|
|
* \param[in] numAxes The number of axes that this controller supports
|
|
* \param[in] idlePoll Poll interval when idle in microseconds
|
|
* \param[in] busyPoll Poll interval when moving in microSeconds
|
|
*/
|
|
extern "C" int
|
|
MasterMACSCreateController(const char *portName,
|
|
const char *MasterMACSPortName, int numAxes, int idlePoll, int busyPoll)
|
|
{
|
|
MasterMACSController *pMasterMACSController
|
|
= new MasterMACSController(portName, MasterMACSPortName, numAxes, idlePoll, busyPoll);
|
|
pMasterMACSController = 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
|
|
MasterMACSController::report(FILE * fp, int level)
|
|
{
|
|
fprintf(fp, "MasterMACS motor driver %s, numAxes=%d\n",
|
|
this->portName, numAxes_);
|
|
|
|
// Call the base class method
|
|
asynMotorController::report(fp, level);
|
|
}
|
|
|
|
/** Returns a pointer to an MasterMACSAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
|
MasterMACSAxis *MasterMACSController::getAxis(asynUser * pasynUser)
|
|
{
|
|
return static_cast <
|
|
MasterMACSAxis * >(asynMotorController::getAxis(pasynUser));
|
|
}
|
|
|
|
/** Returns a pointer to an MasterMACSAxis object.
|
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
|
* \param[in] axisNo Axis index number. */
|
|
MasterMACSAxis *MasterMACSController::getAxis(int axisNo)
|
|
{
|
|
return static_cast <
|
|
MasterMACSAxis * >(asynMotorController::getAxis(axisNo));
|
|
}
|
|
|
|
/**
|
|
* send a command to the MasterMACS 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
|
|
MasterMACSController::transactController(int axisNo,
|
|
char command[COMLEN],
|
|
char reply[COMLEN])
|
|
{
|
|
asynStatus status;
|
|
size_t in, out;
|
|
int reason, len, idx, lenPayload;
|
|
unsigned int i;
|
|
char *mmacsData =
|
|
NULL, ackchar, mmacsResponse[COMLEN], hexResponse[256];
|
|
SINQAxis *axis = getAxis(axisNo);
|
|
|
|
pasynOctetSyncIO->flush(pasynUserController_);
|
|
|
|
/* read with a short timeout in order to remove duplicate messages
|
|
* from the line. This also serves to slow down communication
|
|
*/
|
|
pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .05, &in, &reason);
|
|
|
|
/* pack data for MasterMACS */
|
|
len = strlen(command) + 6;
|
|
mmacsData = (char *) malloc(len * sizeof(char));
|
|
if (!mmacsData) {
|
|
errlogSevPrintf(errlogMajor,
|
|
"Failed to allocate memory in MasterMACSController::transactController");
|
|
return asynError;
|
|
}
|
|
mmacsData[0] = 0x05;
|
|
mmacsData[1] = (char) (len - 2);
|
|
mmacsData[2] = 0;
|
|
mmacsData[3] = 0x19;
|
|
memcpy(mmacsData + 4, command, strlen(command) * sizeof(char));
|
|
mmacsData[len - 2] = 0x0D;
|
|
/* 0x03 is appended by asyn */
|
|
|
|
/* send the stuff away ... */
|
|
if(debug) {
|
|
errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
|
|
}
|
|
|
|
status =
|
|
pasynOctetSyncIO->writeRead(pasynUserController_, mmacsData,
|
|
len - 1, mmacsResponse, 35, 5., &out,
|
|
&in, &reason);
|
|
if (status != asynSuccess) {
|
|
if (axis != NULL) {
|
|
errlogSevPrintf(errlogMajor,
|
|
"Lost connection to motor controller,a axis %d, reason %d",
|
|
axisNo, reason);
|
|
axis->updateMsgTxtFromDriver
|
|
("Lost connection to motor controller");
|
|
return status;
|
|
}
|
|
errlogSevPrintf(errlogMajor,
|
|
"Lost connection to motor controller without axis, reason %d",
|
|
reason);
|
|
return status;
|
|
}
|
|
free(mmacsData);
|
|
|
|
/* format and print the response in hex for debugging purposes */
|
|
if(debug) {
|
|
for(i = 0, idx = 0; i < in; i++){
|
|
sprintf(hexResponse + idx, "%02x ", (unsigned int)mmacsResponse[i]);
|
|
idx = strlen(hexResponse);
|
|
}
|
|
errlogSevPrintf(errlogMajor,"Reply in hex: %s\n", hexResponse);
|
|
}
|
|
|
|
/* Here we have read the data from the MasterMACS. We proceed to extract
|
|
* the payload and the state of the ACK byte and place that reply into data
|
|
*/
|
|
if ((in < 33)) {
|
|
errlogSevPrintf(errlogMajor,
|
|
"MasterMACS only sent %d bytes, 34 expected",
|
|
(int) in);
|
|
return asynError;
|
|
}
|
|
/*
|
|
* Unpack the MasterMACS message, start by finding <CR>
|
|
*/
|
|
for (i = 4, idx = 0; i < 34; i++) {
|
|
if (mmacsResponse[i] == 0x0d) {
|
|
idx = i;
|
|
break;
|
|
}
|
|
}
|
|
//errlogSevPrintf(errlogMajor, "Found <CR> at %d", idx);
|
|
|
|
/* one character before <CR> is the ACK, if it is there */
|
|
ackchar = mmacsResponse[idx - 1];
|
|
if (ackchar == 0x06) {
|
|
strcpy(reply, "ACK:");
|
|
} else if (ackchar == 0x15) {
|
|
strcpy(reply, "NAK:");
|
|
errlogSevPrintf(errlogMajor,
|
|
"MasterMACS responded with NAK to %s\n", command);
|
|
status = asynError;
|
|
} else {
|
|
/* the MasterMACS does not always send a ACK/NAK on read */
|
|
strcpy(reply, "NON:");
|
|
}
|
|
//errlogSevPrintf(errlogMajor, "Reply after testing ACK byte: %s ", reply);
|
|
|
|
/* copy data */
|
|
lenPayload = idx - 4;
|
|
memcpy(reply + 4, mmacsResponse + 4, lenPayload);
|
|
reply[4 + lenPayload] = (char) 0;
|
|
if(debug) {
|
|
errlogSevPrintf(errlogMajor, "Completed reply at end of transactController: %s ", reply);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus
|
|
MasterMACSController::writeInt32(asynUser * pasynUser,
|
|
epicsInt32 value)
|
|
{
|
|
int function = pasynUser->reason;
|
|
asynStatus status = asynSuccess;
|
|
MasterMACSAxis *pAxis = NULL;
|
|
char command[64] = { 0 };
|
|
char response[64] = { 0 };
|
|
int devStatus;
|
|
bool moving;
|
|
time_t startTime;
|
|
|
|
pAxis = (MasterMACSAxis *) this->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 */
|
|
pAxis->setIntegerParam(function, value);
|
|
|
|
if(function == motorStop_){
|
|
errlogPrintf("Stop called with value %d\n", value);
|
|
double accel;
|
|
getDoubleParam(pAxis->axisNo_, motorAccel_, &accel);
|
|
status = pAxis->stop(accel);
|
|
return status;
|
|
}
|
|
if (function == enableAxis_) {
|
|
/*
|
|
* Read the status in order to prevent execssive commands
|
|
*/
|
|
devStatus = pAxis->readStatus();
|
|
if (value == 1 && !pAxis->isOn(devStatus) ) {
|
|
/* download parameters, does not work as of now */
|
|
/*
|
|
sprintf(command, "%dS85=1.", pAxis->axisNo_);
|
|
status = transactController(pAxis->axisNo_, command, response);
|
|
*/
|
|
|
|
/* actually enable */
|
|
sprintf(command, "%dS04=1", pAxis->axisNo_);
|
|
status = transactController(pAxis->axisNo_, command, response);
|
|
} else if(pAxis->isOn(devStatus)) {
|
|
// only send command when necessary
|
|
sprintf(command, "%dS04=0", pAxis->axisNo_);
|
|
status = transactController(pAxis->axisNo_, command, response);
|
|
} else {
|
|
// nothing to do
|
|
return status;
|
|
}
|
|
if (status == asynSuccess) {
|
|
pAxis->updateMsgTxtFromDriver("");
|
|
} else {
|
|
errlogPrintf("MMACS: Failure to enable or disable axis %d",
|
|
pAxis->axisNo_);
|
|
}
|
|
startTime = time(NULL);
|
|
while(time(NULL) < startTime + 2.){
|
|
// wait for the change to happen
|
|
devStatus = pAxis->readStatus();
|
|
if(pAxis->isOn(devStatus) == value){
|
|
pAxis->active = true;
|
|
pAxis->poll(&moving); // to update the Enable_RBV field
|
|
pAxis->active = false;
|
|
return asynSuccess;
|
|
}
|
|
usleep(200);
|
|
}
|
|
errlogPrintf("MMACS: Failed to enable motor %d within 2 seconds\n", pAxis->axisNo_);
|
|
}
|
|
return asynMotorController::writeInt32(pasynUser, value);
|
|
}
|
|
|
|
|
|
// These are the MasterMACSAxis methods
|
|
|
|
/** Creates a new MasterMACSAxis object.
|
|
* \param[in] pC Pointer to the MasterMACSController to which this axis belongs.
|
|
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
|
*
|
|
* Initializes register numbers, etc.
|
|
*/
|
|
MasterMACSAxis::MasterMACSAxis(MasterMACSController * pC, int axisNo):SINQAxis(pC, axisNo),
|
|
pC_
|
|
(pC)
|
|
{
|
|
hasStarted = false;
|
|
active = false;
|
|
}
|
|
|
|
/** 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 MasterMACSAxis::report(FILE * fp, int level)
|
|
{
|
|
if (level > 0) {
|
|
fprintf(fp, " axis %d\n", axisNo_);
|
|
}
|
|
}
|
|
|
|
int MasterMACSAxis::readStatus()
|
|
{
|
|
char command[COMLEN], reply[COMLEN], *pPtr;
|
|
float fval;
|
|
int status;
|
|
|
|
/*
|
|
* The MasterMACS sends invalid responses with a low frequency.
|
|
* Therefore I send cached status responses in such a case in order
|
|
* to help the logic evrywhere else in the code.
|
|
*/
|
|
sprintf(command, "%dR10", axisNo_);
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
if (status == asynError) {
|
|
return oldStatus;
|
|
}
|
|
pPtr = strstr(reply, "=");
|
|
if(pPtr) {
|
|
sscanf(pPtr + 1, "%f", &fval);
|
|
oldStatus = (int) fval;
|
|
return oldStatus;
|
|
} else {
|
|
errlogPrintf("MMACS: invalid status reponse %s on axis %d\n", reply, axisNo_);
|
|
return oldStatus;
|
|
}
|
|
}
|
|
|
|
|
|
int MasterMACSAxis::isOn(int axisStatus)
|
|
{
|
|
if (CHECK_BIT(axisStatus, 0) && CHECK_BIT(axisStatus, 1)) {
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
asynStatus
|
|
MasterMACSAxis::move(double position, int relative, double minVelocity,
|
|
double maxVelocity, double acceleration)
|
|
{
|
|
asynStatus status = asynSuccess;
|
|
char command[COMLEN], reply[COMLEN];
|
|
int devStatus;
|
|
|
|
//errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity);
|
|
memset(command, 0, COMLEN * sizeof(char));
|
|
|
|
/* clear motor error message */
|
|
updateMsgTxtFromDriver("");
|
|
|
|
/*
|
|
* reset error code
|
|
*/
|
|
if(errorCodeFound){
|
|
sprintf(command, "%dS17=0", axisNo_);
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
errorCodeFound = 0;
|
|
}
|
|
|
|
/*
|
|
* only start if the thing is On
|
|
*/
|
|
devStatus = readStatus();
|
|
|
|
/*
|
|
* set speed
|
|
*/
|
|
/* temporarily disabled in order to get the basic logic right
|
|
sprintf(command, "%dS05=%f", axisNo_, maxVelocity / 1000);
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
*/
|
|
|
|
if (relative) {
|
|
position += this->position;
|
|
}
|
|
|
|
if(isOn(devStatus) && active == false) {
|
|
errlogPrintf("Starting axis %d with destination %f\n", axisNo_,
|
|
position / 1000.);
|
|
|
|
/* send target position */
|
|
sprintf(command, "%dS02= %.3f", axisNo_, position / 1000.);
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
if (status == asynError) {
|
|
errlogPrintf("MMACS: failed to set target on %d\n", axisNo_);
|
|
updateMsgTxtFromDriver("Failed to send target position");
|
|
return status;
|
|
}
|
|
|
|
/* send move command */
|
|
sprintf(command, "%dS00=1", axisNo_);
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
if (status == asynError) {
|
|
errlogPrintf("MMACS: failed to start axis %d\n", axisNo_);
|
|
updateMsgTxtFromDriver("Failed to start axis");
|
|
return status;
|
|
}
|
|
hasStarted = true;
|
|
homing = 0;
|
|
active = true;
|
|
usleep(500);
|
|
lastPositionUpdate = time(NULL);
|
|
} else {
|
|
errlogPrintf("MMACS: axis %d disabled, cannot start\n", axisNo_);
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus
|
|
MasterMACSAxis::home(double minVelocity, double maxVelocity,
|
|
double acceleration, int forwards)
|
|
{
|
|
asynStatus status;
|
|
char command[COMLEN], reply[COMLEN];
|
|
int devStatus;
|
|
|
|
memset(command, 0, COMLEN * sizeof(char));
|
|
|
|
/*
|
|
* test if the thing is On
|
|
*/
|
|
devStatus = readStatus();
|
|
updateMsgTxtFromDriver("");
|
|
|
|
if(isOn(devStatus)){
|
|
sprintf(command, "%dS00=9", axisNo_);
|
|
homing = 1;
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
hasStarted = true;
|
|
active = true;
|
|
lastPositionUpdate = time(NULL);
|
|
return status;
|
|
} else {
|
|
errlogPrintf("MMACS: cannot home disabled axis %d\n", axisNo_);
|
|
return asynError;
|
|
}
|
|
}
|
|
|
|
asynStatus
|
|
MasterMACSAxis::moveVelocity(double minVelocity, double maxVelocity,
|
|
double acceleration)
|
|
{
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
errlogSevPrintf(errlogMajor,
|
|
"This controller does not support the jog feature");
|
|
return asynError;
|
|
}
|
|
|
|
asynStatus MasterMACSAxis::stop(double acceleration)
|
|
{
|
|
asynStatus status = asynSuccess;
|
|
char command[COMLEN], reply[COMLEN];
|
|
int devStatus;
|
|
|
|
memset(command, 0, COMLEN * sizeof(char));
|
|
|
|
devStatus = readStatus();
|
|
if(!CHECK_BIT(devStatus, 10)) {
|
|
// only try to stop when running ...
|
|
sprintf(command, "%dS00=8", axisNo_);
|
|
status = pC_->transactController(axisNo_, command, reply);
|
|
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
|
|
updateMsgTxtFromDriver("Axis interrupted");
|
|
}
|
|
return status;
|
|
}
|
|
|
|
asynStatus MasterMACSAxis::setPosition(double position)
|
|
{
|
|
setIntegerParam(pC_->motorStatusProblem_, true);
|
|
errlogSevPrintf(errlogMajor,
|
|
"This controller does not support setting position");
|
|
updateMsgTxtFromDriver("Controller does not support setPosition");
|
|
return asynError;
|
|
}
|
|
|
|
asynStatus MasterMACSAxis::setClosedLoop(bool closedLoop)
|
|
{
|
|
/*
|
|
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 MasterMACSAxis::poll(bool * moving)
|
|
{
|
|
asynStatus comStatus = asynSuccess;
|
|
char command[COMLEN], reply[COMLEN], *pPtr, buffer[80];
|
|
unsigned int errCode, derCode, devStatus;
|
|
float errStatus;
|
|
struct tm* tm_info;
|
|
time_t timer;
|
|
|
|
/*
|
|
* EPICS always polls all motors on a controller when one motor is active.
|
|
* In order to reduce load on the controller we check if we are active and
|
|
* if not return old stuff
|
|
*/
|
|
if(active == false && time(NULL) < lastPoll + pC_->idlePollPeriod_) {
|
|
*moving = false;
|
|
return asynSuccess;
|
|
}
|
|
|
|
timer = time(NULL);
|
|
tm_info = localtime(&timer);
|
|
|
|
strftime(buffer, 26, "%Y-%m-%d %H:%M:%S", tm_info);
|
|
errlogPrintf("poll called at %s on axis %d \n",
|
|
buffer, axisNo_ );
|
|
lastPoll = time(NULL);
|
|
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
memset(command, 0, COMLEN * sizeof(char));
|
|
|
|
// Read the current motor position
|
|
sprintf(command, "%dR12", axisNo_);
|
|
comStatus = pC_->transactController(axisNo_, command, reply);
|
|
if(comStatus != asynError) {
|
|
pPtr = strstr(reply, "=");
|
|
if(pPtr) {
|
|
sscanf(pPtr + 1, "%lf", &position);
|
|
setDoubleParam(pC_->motorPosition_, position * 1000.);
|
|
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
|
|
/*
|
|
* keep track of position in order to check for a stuck motor later
|
|
*/
|
|
if(ABS(position - oldPosition) > 1){
|
|
oldPosition = position;
|
|
lastPositionUpdate = time(NULL);
|
|
}
|
|
} else {
|
|
errlogPrintf("MMACS: Invalid response asking position on %d\n", axisNo_);
|
|
}
|
|
} else {
|
|
errlogPrintf("MMACS: communication problem reading axis %d position\n", axisNo_);
|
|
}
|
|
|
|
// Read the overall status of this motor */
|
|
devStatus = readStatus();
|
|
if(debug) {
|
|
errlogPrintf("Axis %d, position %lf, devStatus %d\n", axisNo_,
|
|
position, devStatus);
|
|
}
|
|
|
|
// Check for the thing being in a bad state
|
|
if(CHECK_BIT(devStatus, 6)) {
|
|
*moving = false;
|
|
active = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
updateMsgTxtFromDriver("AXIS dead");
|
|
goto skip;
|
|
}
|
|
|
|
|
|
setIntegerParam(pC_->enableAxis_, isOn(devStatus));
|
|
setIntegerParam(pC_->axisEnabled_, isOn(devStatus));
|
|
|
|
// Check if the motor is disabled
|
|
if (!isOn(devStatus)) {
|
|
updateMsgTxtFromDriver("Axis disabled");
|
|
*moving = false;
|
|
active = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
goto skip;
|
|
}
|
|
|
|
/*
|
|
* if the motor has never run, the status bit 10 is invalid
|
|
*/
|
|
if (!hasStarted) {
|
|
*moving = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
active = false;
|
|
goto skip;
|
|
}
|
|
|
|
/*
|
|
* We may have a valid status bit...
|
|
*/
|
|
if (!CHECK_BIT(devStatus, 10)) {
|
|
/* we are still creeping along .... */
|
|
*moving = true;
|
|
setIntegerParam(pC_->motorStatusDone_, false);
|
|
if(time(NULL) > lastPositionUpdate + 120) {
|
|
// we are detecting a stuck motor
|
|
errlogPrintf("MMACS: axis %d is STUCK!!\n", axisNo_);
|
|
updateMsgTxtFromDriver("Axis STUCK!!");
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
*moving = false;
|
|
active = false;
|
|
}
|
|
goto skip;
|
|
}
|
|
|
|
|
|
/*we are done moving */
|
|
*moving = false;
|
|
active = false;
|
|
setIntegerParam(pC_->motorStatusDone_, true);
|
|
updateMsgTxtFromDriver("");
|
|
|
|
/* when homing, set the proper flag */
|
|
if (homing) {
|
|
setIntegerParam(pC_->motorStatusAtHome_, true);
|
|
}
|
|
|
|
/* check for limit switches*/
|
|
setIntegerParam(pC_->motorStatusLowLimit_, false);
|
|
setIntegerParam(pC_->motorStatusHighLimit_, false);
|
|
if (CHECK_BIT(devStatus, 11)) {
|
|
errlogSevPrintf(errlogMajor, "Limit bit in status code %d", axisNo_);
|
|
/* guessing which limit has been hit ... */
|
|
if (position > 0) {
|
|
updateMsgTxtFromDriver("Hit positive limit switch");
|
|
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
|
} else {
|
|
updateMsgTxtFromDriver("Hit negative limit switch");
|
|
setIntegerParam(pC_->motorStatusLowLimit_, true);
|
|
}
|
|
goto skip;
|
|
}
|
|
|
|
/* check for error conditions */
|
|
if (CHECK_BIT(devStatus, 3)) {
|
|
/* read error codes */
|
|
sprintf(command, "%dR11", axisNo_);
|
|
comStatus = pC_->transactController(axisNo_, command, reply);
|
|
if (comStatus != asynError) {
|
|
pPtr = strstr(reply, "=");
|
|
if(pPtr) {
|
|
sscanf(pPtr + 1, "%f", &errStatus);
|
|
errCode = (unsigned int) errStatus;
|
|
|
|
} else {
|
|
errlogPrintf("MMACS: axis %d received invalid reply asking for error code \n", axisNo_);
|
|
errCode = 0;
|
|
goto skip;
|
|
}
|
|
} else {
|
|
errlogPrintf("MMACS: axis %d failed reading error code \n", axisNo_);
|
|
goto skip;
|
|
}
|
|
|
|
sprintf(command, "%dR18", axisNo_);
|
|
comStatus = pC_->transactController(axisNo_, command, reply);
|
|
if (comStatus != asynError) {
|
|
pPtr = strstr(reply, "=");
|
|
if(pPtr) {
|
|
sscanf(pPtr + 1, "%f", &errStatus);
|
|
derCode = (unsigned int) errStatus;
|
|
} else {
|
|
errlogPrintf("MMACS: malformed reply for R18: %s, on axis %d\n", reply, axisNo_);
|
|
derCode = 0;
|
|
goto skip;
|
|
}
|
|
} else {
|
|
errlogPrintf("MMACS: axis %d failed reading extended error code R18 \n", axisNo_);
|
|
goto skip;
|
|
}
|
|
|
|
if(debug) {
|
|
errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
|
|
errCode, derCode);
|
|
}
|
|
|
|
if (errCode == 0) {
|
|
errlogSevPrintf(errlogMajor,
|
|
"Fault bit in status code, but no error code on %d\n", axisNo_);
|
|
updateMsgTxtFromDriver ("Fault bit in status code without error code");
|
|
goto skip;
|
|
}
|
|
errorCodeFound = 1;
|
|
|
|
if (CHECK_BIT(errCode, 0)) {
|
|
errlogSevPrintf(errlogMajor, "CAN error on %d", axisNo_);
|
|
updateMsgTxtFromDriver("CAN error");
|
|
} else if (CHECK_BIT(errCode, 1)) {
|
|
errlogSevPrintf(errlogMajor, "Short circuit on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Short circuit");
|
|
} else if (CHECK_BIT(errCode, 2)) {
|
|
errlogSevPrintf(errlogMajor, "Invalide Setup on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Invalid Setup");
|
|
} else if (CHECK_BIT(errCode, 3)) {
|
|
errlogSevPrintf(errlogMajor, "Control error on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Control error");
|
|
} else if (CHECK_BIT(errCode, 4)) {
|
|
errlogSevPrintf(errlogMajor, "CAN communication error on %d",
|
|
axisNo_);
|
|
updateMsgTxtFromDriver("CAN communication error");
|
|
} else if (CHECK_BIT(errCode, 5)) {
|
|
errlogSevPrintf(errlogMajor, "Feedback error on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Feedback error");
|
|
} else if (CHECK_BIT(errCode, 6)) {
|
|
updateMsgTxtFromDriver("Hit positive limit switch");
|
|
setIntegerParam(pC_->motorStatusHighLimit_, true);
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
} else if (CHECK_BIT(errCode, 7)) {
|
|
updateMsgTxtFromDriver("Hit negative limit switch");
|
|
setIntegerParam(pC_->motorStatusLowLimit_, true);
|
|
setIntegerParam(pC_->motorStatusProblem_, false);
|
|
} else if (CHECK_BIT(errCode, 8)) {
|
|
errlogSevPrintf(errlogMajor, "Over current %d", axisNo_);
|
|
updateMsgTxtFromDriver("Over current");
|
|
} else if (CHECK_BIT(errCode, 9)) {
|
|
errlogSevPrintf(errlogMajor, "I2T protection on %d", axisNo_);
|
|
updateMsgTxtFromDriver("I2t protection");
|
|
} else if (CHECK_BIT(errCode, 10)) {
|
|
errlogSevPrintf(errlogMajor, "Over heated motor on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Motor overheated");
|
|
} else if (CHECK_BIT(errCode, 11)) {
|
|
errlogSevPrintf(errlogMajor, "Over temperature drive on %d",
|
|
axisNo_);
|
|
updateMsgTxtFromDriver("Over temperature drive");
|
|
} else if (CHECK_BIT(errCode, 12)) {
|
|
errlogSevPrintf(errlogMajor, "Over voltage on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Over voltage");
|
|
} else if (CHECK_BIT(errCode, 13)) {
|
|
errlogSevPrintf(errlogMajor, "Under voltage on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Under voltage");
|
|
} else if (CHECK_BIT(errCode, 14)) {
|
|
errlogSevPrintf(errlogMajor, "Command error on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Command error");
|
|
} else if (CHECK_BIT(errCode, 15)) {
|
|
errlogSevPrintf(errlogMajor, "Motor disabled on %d", axisNo_);
|
|
updateMsgTxtFromDriver("Motor disabled");
|
|
}
|
|
}
|
|
|
|
skip:
|
|
callParamCallbacks();
|
|
return comStatus;
|
|
}
|
|
|
|
/** Code for iocsh registration */
|
|
static const iocshArg
|
|
MasterMACSCreateControllerArg0 = { "Port name", iocshArgString };
|
|
|
|
static const iocshArg
|
|
MasterMACSCreateControllerArg1 =
|
|
{ "MasterMACS port name", iocshArgString };
|
|
|
|
static const iocshArg
|
|
MasterMACSCreateControllerArg2 = { "Number of axes", iocshArgInt };
|
|
|
|
static const iocshArg
|
|
MasterMACSCreateControllerArg3 = { "idlePoll", iocshArgInt };
|
|
|
|
static const iocshArg
|
|
MasterMACSCreateControllerArg4 = { "busyPoll", iocshArgInt };
|
|
|
|
|
|
static const iocshArg *const
|
|
MasterMACSCreateControllerArgs[] = { &MasterMACSCreateControllerArg0,
|
|
&MasterMACSCreateControllerArg1,
|
|
&MasterMACSCreateControllerArg2,
|
|
&MasterMACSCreateControllerArg3,
|
|
&MasterMACSCreateControllerArg4
|
|
};
|
|
|
|
static const iocshFuncDef
|
|
MasterMACSCreateControllerDef =
|
|
{ "MasterMACSCreateController", 5, MasterMACSCreateControllerArgs };
|
|
static void MasterMACSCreateContollerCallFunc(const iocshArgBuf * args)
|
|
{
|
|
MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival,
|
|
args[3].ival, args[4].ival);
|
|
}
|
|
|
|
/**
|
|
* C wrapper for the MasterMACSAxis constructor.
|
|
* See MasterMAXSAxis::MasterMACSAxis.
|
|
*
|
|
*/
|
|
asynStatus MasterMACSCreateAxis(const char *MasterMACSPort, /* specify which controller by port name */
|
|
int axis)
|
|
{ /* axis number (start from 1). */
|
|
MasterMACSController *pC;
|
|
MasterMACSAxis *pAxis;
|
|
|
|
static const char *functionName = "MasterMACSCreateAxis";
|
|
|
|
pC = (MasterMACSController *) findAsynPortDriver(MasterMACSPort);
|
|
if (!pC) {
|
|
printf("%s:%s: Error port %s not found\n", "MasterMACSDriver",
|
|
functionName, MasterMACSPort);
|
|
return asynError;
|
|
}
|
|
|
|
pC->lock();
|
|
pAxis = new MasterMACSAxis(pC, axis);
|
|
pAxis = NULL;
|
|
pC->unlock();
|
|
return asynSuccess;
|
|
}
|
|
|
|
/* MasterMACSCreateAxis */
|
|
static const iocshArg
|
|
MasterMACSCreateAxisArg0 = { "Controller port name", iocshArgString };
|
|
|
|
static const iocshArg
|
|
MasterMACSCreateAxisArg1 = { "Axis number", iocshArgInt };
|
|
|
|
static const iocshArg *const
|
|
MasterMACSCreateAxisArgs[] = { &MasterMACSCreateAxisArg0,
|
|
&MasterMACSCreateAxisArg1
|
|
};
|
|
|
|
static const iocshFuncDef
|
|
configMasterMACSAxis =
|
|
{ "MasterMACSCreateAxis", 2, MasterMACSCreateAxisArgs };
|
|
|
|
static void configMasterMACSAxisCallFunc(const iocshArgBuf * args)
|
|
{
|
|
MasterMACSCreateAxis(args[0].sval, args[1].ival);
|
|
}
|
|
|
|
static void MasterMACSRegister(void)
|
|
{
|
|
iocshRegister(&MasterMACSCreateControllerDef,
|
|
MasterMACSCreateContollerCallFunc);
|
|
iocshRegister(&configMasterMACSAxis, configMasterMACSAxisCallFunc);
|
|
}
|
|
|
|
extern "C" {
|
|
epicsExportRegistrar(MasterMACSRegister);
|
|
}
|