lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d44dbaaf0f18d293434d31b9deb1a9fbc5c38f02
sinqepicsapp/sinqEPICSApp/src/pmacAxis.cpp

1756 lines
55 KiB
C++
Raw Blame History

/*******************************************
* pmacAxis.cpp
*
* PMAC Asyn motor based on the
* asynMotorAxis class.
*
* Matthew Pearson
* 23 May 2012
*
* Substantially modified for use at SINQ at PSI.
* The thing with the PMAC's is that they can be programmed.
* This affects also the commands they understand.
* Our PMAC's also do not seem to like to return multiple replies.....
*
* I use a starting flag to catch a peculiar feature of our PMAC implementation:
* when the motor is hung, it wont start. I check for this and cause an alarm.
*
* Another mode where the motor is in trouble is if it stays too long in status
* 5 or 6. I check and cause an alarm in this state too.
*
* Mark Koennecke, February 2013
*
* Modified to use the MsgTxt field for SINQ
*
* Mark Koennecke, January 2019
*
* Add driver for V3 protocol
*
* Michele Brambilla, February 2022
*
* Added driver for special AMOR detector axes
*
* Mark Koennecke, June 2023
*
* Added driver for GirderAxis
*
* Stefan Mathis, July 2024
*
********************************************/
#include <epicsExit.h>
#include <epicsExport.h>
#include <epicsString.h>
#include <epicsThread.h>
#include <epicsTime.h>
#include <errlog.h>
#include <iocsh.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include<algorithm>
#include<cmath>
#include<utility>
#include <memory>
#include "pmacController.h"
#define MULT 1000.
#define IDLEPOLL 2.
#define BUSYPOLL .05
#define ABS(x) (x < 0 ? -(x) : (x))
extern "C" void shutdownCallback(void *pPvt) {
pmacController *pC = static_cast<pmacController *>(pPvt);
pC->lock();
pC->shuttingDown_ = 1;
pC->unlock();
}
// These are the pmacAxis class methods
pmacAxis::pmacAxis(pmacController *pC, int axisNo, bool enable)
: SINQAxis(pC, axisNo), pC_(pC), autoEnable(enable) {
static const char *functionName = "pmacAxis::pmacAxis";
pC_->debugFlow(functionName);
// Initialize non-static data members
setpointPosition_ = 0.0;
encoderPosition_ = 0.0;
currentVelocity_ = 0.0;
velocity_ = 0.0;
accel_ = 0.0;
highLimit_ = 0.0;
lowLimit_ = 0.0;
scale_ = 1;
previous_position_ = 0.0;
previous_direction_ = 0;
axisErrorCount = 0;
startTime = 0;
status6Time = 0;
starting = 0;
homing = 0;
next_poll = -1;
/* Set an EPICS exit handler that will shut down polling before asyn kills the
* IP sockets */
epicsAtExit(shutdownCallback, pC_);
// Do an initial poll to get some values from the PMAC
if (getAxisInitialStatus() != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: getAxisInitialStatus failed to return asynSuccess. "
"Controller: %s, Axis: %d.\n",
functionName, pC_->portName, axisNo_);
}
callParamCallbacks();
/* Wake up the poller task which will make it do a poll,
* updating values for this axis to use the new resolution (stepSize_) */
pC_->wakeupPoller();
}
asynStatus pmacAxis::getAxisInitialStatus(void) {
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
int cmdStatus = 0;
double low_limit = 0.0;
double high_limit = 0.0;
int nvals = 0;
int limVal;
static const char *functionName = "pmacAxis::getAxisInitialStatus";
sprintf(command, "Q%2.2d00", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%lf", &scale_);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: failed to read scale_factor on axis %d.\n",
functionName, axisNo_);
return asynError;
}
sprintf(command, "I%d13", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &limVal);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: failed to read high limit on axis %d.\n",
functionName, axisNo_);
return asynError;
}
high_limit = ((double)limVal) * scale_;
sprintf(command, "I%d14", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &limVal);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: failed to read low limit on axis %d.\n", functionName,
axisNo_);
return asynError;
}
low_limit = ((double)limVal) * scale_;
char message[132];
sprintf(message, "scale_factor = %lf, high = %lf, low = %lf", scale_,
high_limit, low_limit);
pC_->debugFlow(message);
setIntegerParam(pC_->motorStatusHasEncoder_, 1);
/*
In some configurations at SINQ, the high and low limits are interchanged in
the motor controller. This messy bit of code takes care of this messy
electronics configuration.
*/
if (high_limit > low_limit) {
setDoubleParam(pC_->motorLowLimit_, low_limit);
setDoubleParam(pC_->motorHighLimit_, high_limit);
} else {
setDoubleParam(pC_->motorLowLimit_, high_limit);
setDoubleParam(pC_->motorHighLimit_, low_limit);
}
// Enable the axis. After startup, the axis are disabled on the controller...
// Warning: Selene lift axis should not be automatically enabled
if (autoEnable) {
sprintf(command, "M%2.2d14=1\n", axisNo_);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Enable axis %d: %s",
axisNo_, command);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
if (cmdStatus) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: enabling axis %d failed.\n", functionName,
axisNo_);
return asynError;
}
}
callParamCallbacks();
return asynSuccess;
}
pmacAxis::~pmacAxis() {
// Destructor
}
asynStatus pmacAxis::move(double position, int relative, double min_velocity,
double max_velocity, double acceleration) {
asynStatus status = asynError;
static const char *functionName = "pmacAxis::move";
double realPosition;
updateMsgTxtFromDriver("");
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
pC_->debugFlow(functionName);
if (relative) {
realPosition = previous_position_ + position / MULT;
} else {
realPosition = position / MULT;
}
startTime = time(NULL);
status6Time = 0;
starting = 1;
/* set speed first */
sprintf(command, "Q%d04=%f", axisNo_, max_velocity);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
/* send drive command */
sprintf(command, "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", axisNo_, axisNo_,
realPosition, axisNo_);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
next_poll = -1;
return status;
}
asynStatus pmacAxis::home(double min_velocity, double max_velocity,
double acceleration, int forwards) {
asynStatus status = asynError;
char command[128] = {0};
char response[128] = {0};
static const char *functionName = "pmacAxis::home";
pC_->debugFlow(functionName);
updateMsgTxtFromDriver("");
sprintf(command, "M%2.2d=9", axisNo_);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
homing = 1;
next_poll = time(NULL) + BUSYPOLL;
return status;
}
asynStatus pmacAxis::moveVelocity(double min_velocity, double max_velocity,
double acceleration) {
asynStatus status = asynError;
static const char *functionName = "pmacAxis::moveVelocity";
pC_->debugFlow(functionName);
return asynError; // can we do this, actually?
// status = pC_->lowLevelWriteRead(axisNo_,command, response);
return status;
}
asynStatus pmacAxis::setPosition(double position) {
// int status = 0;
static const char *functionName = "pmacAxis::setPosition";
errlogPrintf("executing : %s\n", functionName);
pC_->debugFlow(functionName);
// Cannot do this.
return asynSuccess;
}
asynStatus pmacAxis::stop(double acceleration) {
asynStatus status = asynSuccess;
static const char *functionName = "pmacAxis::stopAxis";
bool moving = false;
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
this->getAxisStatus(&moving);
if(moving) {
// only send a stop when actually moving
sprintf(command, "M%2.2d=8", axisNo_);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
}
return status;
}
asynStatus pmacAxis::poll(bool *moving) {
int status = 0;
static const char *functionName = "pmacAxis::poll";
char message[132];
// Protect against excessive polling
if (time(NULL) < next_poll) {
return asynSuccess;
}
sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_);
pC_->debugFlow(message);
// Now poll axis status
if ((status = this->getAxisStatus(moving)) != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: getAxisStatus failed to return asynSuccess. Controller: %s, "
"Axis: %d.\n",
functionName, pC_->portName, axisNo_);
}
if (*moving) {
next_poll = time(NULL) + BUSYPOLL;
} else {
next_poll = time(NULL) + IDLEPOLL;
}
callParamCallbacks();
return status ? asynError : asynSuccess;
}
static char *translateAxisError(int axErr) {
switch (axErr) {
case 0:
return strdup("");
break;
case 1:
return strdup("limit violation");
break;
case 2:
case 3:
case 4:
return strdup("jog error");
break;
case 5:
return strdup("command not allowed");
break;
case 6:
return strdup("watchdog triggered");
break;
case 7:
return strdup("current limit reached");
break;
case 8:
case 9:
return strdup("air cushion error");
break;
case 10:
return strdup("MCU limit reached");
break;
case 11:
return strdup("following error triggered");
break;
case 12:
return strdup("EMERGENCY STOP activated");
break;
case 13:
return strdup("Driver electronics error");
break;
case 15:
return strdup("Motor blocked");
break;
default:
return strdup("Unknown axis error");
break;
}
}
asynStatus pmacAxis::getAxisStatus(bool *moving) {
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
int cmdStatus = 0;
;
int done = 0, posChanging = 0;
double position = 0;
int nvals = 0;
int axisProblemFlag = 0, axStat = 0;
epicsUInt32 axErr = 0, highLim = 0, lowLim = 0;
char message[132], *axMessage;
/* read our status items one by one: our PMAC does not seem to give multiple
* responses ..*/
sprintf(command, "P%2.2d01", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &axErr);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read axis Error Status: "
"%d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver("Cannot read Axis Error Status");
}
sprintf(command, "Q%2.2d10", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%lf", &position);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read position Status: "
"%d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver("Cannot read Axis position");
}
sprintf(command, "P%2.2d00", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &axStat);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read axis status, Status: "
"%d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver("Cannot read Axis Status");
}
sprintf(command, "M%2.2d21", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &highLim);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read high limit flag Status: "
"%d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
}
sprintf(command, "M%2.2d22", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &lowLim);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to low limit flag Status: "
"%d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
}
int direction = 0;
setDoubleParam(pC_->motorPosition_, position * MULT);
setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
/* Use previous position and current position to calculate direction.*/
if ((position - previous_position_) > 0) {
direction = 1;
} else if (position - previous_position_ == 0.0) {
direction = previous_direction_;
} else {
direction = 0;
}
setIntegerParam(pC_->motorStatusDirection_, direction);
/*Store position to calculate direction for next poll.*/
previous_position_ = position;
previous_direction_ = direction;
// errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat,
// axErr, position);
/* are we done? */
if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) {
done = 1;
} else {
starting = 0;
done = 0;
}
if (starting && time(NULL) > startTime + 10) {
/*
did not start in 10 seconds: messed up: cause an alarm
*/
done = 1;
*moving = false;
setIntegerParam(pC_->motorStatusMoving_, false);
setIntegerParam(pC_->motorStatusDone_, true);
errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_);
updateMsgTxtFromDriver("Axis did not start within 10 seconds");
starting = 0;
return asynSuccess;
}
/*
code to test against too long status 5 or 6
*/
// if (axStat == 5 || axStat == 6) {
// if (status6Time == 0) {
// status6Time = time(NULL);
// statusPos = position;
// } else {
// if (time(NULL) > status6Time + 60) {
///* trigger error only when not moving */
// if (abs(position - statusPos) < .1) {
// done = 1;
//*moving = false;
// setIntegerParam(pC_->motorStatusMoving_, false);
// setIntegerParam(pC_->motorStatusDone_, true);
// setIntegerParam(pC_->motorStatusProblem_, true);
// errlogPrintf("Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n",
// axisNo_);
// updateMsgTxtFromDriver("Axis stayed in 5,6 for more then 60 seconds:
// BROKEN");
// status6Time = 0;
// return asynSuccess;
// } else {
// status6Time = time(NULL);
// statusPos = position;
// }
// }
// }
//}
if (!done) {
*moving = true;
setIntegerParam(pC_->motorStatusMoving_, true);
setIntegerParam(pC_->motorStatusDone_, false);
} else {
*moving = false;
setIntegerParam(pC_->motorStatusMoving_, false);
setIntegerParam(pC_->motorStatusDone_, true);
if (homing) {
setIntegerParam(pC_->motorStatusHomed_, done);
homing = 0;
}
}
sprintf(message, "poll results: axis %d, status %d, axErr %d, done = %d",
axisNo_, axStat, axErr, done);
pC_->debugFlow(message);
/* search for trouble */
if (highLim) {
setIntegerParam(pC_->motorStatusHighLimit_, true);
updateMsgTxtFromDriver("High Limit Hit");
} else {
setIntegerParam(pC_->motorStatusHighLimit_, false);
}
if (lowLim) {
setIntegerParam(pC_->motorStatusLowLimit_, true);
updateMsgTxtFromDriver("Low Limit Hit");
} else {
setIntegerParam(pC_->motorStatusLowLimit_, false);
}
if (axErr == 11) {
setIntegerParam(pC_->motorStatusFollowingError_, true);
updateMsgTxtFromDriver("Following error triggered");
} else {
setIntegerParam(pC_->motorStatusFollowingError_, false);
}
/* Set any axis specific general problem bits. */
if (axErr != 0) {
axisProblemFlag = 1;
if (axisErrorCount < 10) {
axMessage = translateAxisError(axErr);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error "
"code %d, translated: %s:, status code = %d\n",
axisNo_, axErr, axMessage, axStat);
snprintf(message, sizeof(message), "PMAC Axis error: %s", axMessage);
updateMsgTxtFromDriver(message);
if (axMessage != NULL) {
free(axMessage);
}
axisErrorCount++;
} else if (axisErrorCount == 10) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Suppressing further axis error messages\n");
axisErrorCount++;
}
} else {
axisProblemFlag = 0;
axisErrorCount = 0;
}
setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
return asynSuccess;
}
asynStatus pmacHRPTAxis::getAxisStatus(bool *moving) {
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
int cmdStatus = 0, nvals, crashSignal;
asynStatus result = pmacV3Axis::getAxisStatus(moving);
sprintf(command, "P%2.2d53", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &crashSignal);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read crash flag Status: "
"%d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
}
if (crashSignal == 1) {
updateMsgTxtFromDriver("HSC: HRPT Slit Crash!!");
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: HRPT Slit CRASH detected");
*moving = false;
setIntegerParam(pC_->motorStatusMoving_, false);
setIntegerParam(pC_->motorStatusDone_, true);
}
return result;
}
asynStatus pmacAxis::enable(int on) {
static const char *functionName = "pmacAxis::enable";
pC_->debugFlow(functionName);
// Cannot do this.
return asynSuccess;
}
/*================================= SeleneAxis code
* ======================================================*/
SeleneAxis::SeleneAxis(SeleneController *pC, int axisNo, double limitTarget)
: pmacAxis(pC, axisNo, false) {
static const char *functionName = "SeleneAxis::SeleneAxis";
pC_->debugFlow(functionName);
// Initialize non-static data members
setpointPosition_ = 0.0;
encoderPosition_ = 0.0;
currentVelocity_ = 0.0;
velocity_ = 0.0;
accel_ = 0.0;
highLimit_ = 0.0;
lowLimit_ = 0.0;
scale_ = 1;
previous_position_ = 0.0;
previous_direction_ = 0;
axisErrorCount = 0;
startTime = 0;
status6Time = 0;
starting = 0;
homing = 0;
/* Set an EPICS exit handler that will shut down polling before asyn kills the
* IP sockets */
epicsAtExit(shutdownCallback, pC_);
// Do an initial poll to get some values from the PMAC
if (getSeleneAxisInitialStatus() != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: getAxisInitialStatus failed to return asynSuccess. "
"Controller: %s, Axis: %d.\n",
functionName, pC_->portName, axisNo_);
}
this->limitTarget = limitTarget;
callParamCallbacks();
/* Wake up the poller task which will make it do a poll,
* updating values for this axis to use the new resolution (stepSize_) */
pC_->wakeupPoller();
}
/*-------------------------------------------------------------------------------------------*/
asynStatus SeleneAxis::getSeleneAxisInitialStatus(void) {
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
int cmdStatus = 0;
double low_limit = 0.0;
double high_limit = 0.0;
int nvals = 0;
int limVal;
static const char *functionName = "pmacAxis::getAxisInitialStatus";
sprintf(command, "Q%2.2d00", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%lf", &scale_);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: failed to read scale_factor on axis %d.\n",
functionName, axisNo_);
return asynError;
}
sprintf(command, "I%d13", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &limVal);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: failed to read high limit on axis %d.\n",
functionName, axisNo_);
return asynError;
}
high_limit = ((double)limVal) * scale_;
sprintf(command, "I%d14", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d", &limVal);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: Error: failed to read low limit on axis %d.\n", functionName,
axisNo_);
return asynError;
}
low_limit = ((double)limVal) * scale_;
char message[132];
sprintf(message, "scale_factor = %lf, high = %lf, low = %lf", scale_,
high_limit, low_limit);
pC_->debugFlow(message);
setIntegerParam(pC_->motorStatusHasEncoder_, 1);
/*
In some configurations at SINQ, the high and low limits are interchanged in
the motor controller. This messy bit of code takes care of this messy
electronics configuration.
*/
if (high_limit > low_limit) {
setDoubleParam(pC_->motorLowLimit_, low_limit);
setDoubleParam(pC_->motorHighLimit_, high_limit);
} else {
setDoubleParam(pC_->motorLowLimit_, high_limit);
setDoubleParam(pC_->motorHighLimit_, low_limit);
}
callParamCallbacks();
return asynSuccess;
}
/*----------------------------------------------------------------------------------------------------------*/
asynStatus SeleneAxis::home(double min_velocity, double max_velocity,
double acceleration, int forwards) {
asynStatus status = asynError;
static const char *functionName = "SeleneAxis::home";
pC_->debugFlow(functionName);
updateMsgTxtFromDriver("Cannot home on this axis type");
return status;
}
/*----------------------------------------------------------------------------------------------------------------*/
asynStatus SeleneAxis::move(double position, int relative, double min_velocity,
double max_velocity, double acceleration) {
asynStatus status = asynError;
static const char *functionName = "SeleneAxis::move";
double realPosition;
updateMsgTxtFromDriver("");
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
pC_->debugFlow(functionName);
if (relative) {
realPosition = previous_position_ + position / MULT;
} else {
realPosition = position / MULT;
}
startTime = time(NULL);
status6Time = 0;
starting = 1;
/*
Run into limit when asked for by a suitable position, else
position absolutely
*/
if (ABS(realPosition - limitTarget) < .05) {
sprintf(command, "P%d50=3", axisNo_);
} else {
sprintf(command, "Q%d51=%f P%x50=1", axisNo_, realPosition, axisNo_);
}
errlogPrintf("Sending drive command: %s\n", command);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
return status;
}
/*----------------------------------------------------------------------------------------------------*/
asynStatus SeleneAxis::setPosition(double position) {
asynStatus status = asynError;
static const char *functionName = "SeleneAxis::setPosition";
char command[128] = {0};
char response[32] = {0};
pC_->debugFlow(functionName);
snprintf(command, sizeof(command), "Q%d59=%f", axisNo_, position / MULT);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
errlogPrintf("Sending set position : %s\n", command);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_FLOW,
"Setting Position on %d to value %f, command: %s", axisNo_,
position / MULT, command);
next_poll = -1;
return status;
}
/*======================================= Selene Lift Axis Code
* ===========================================*/
asynStatus LiftAxis::move(double position, int relative, double min_velocity,
double max_velocity, double acceleration) {
asynStatus status = asynError;
static const char *functionName = "LiftAxis::move";
double realPosition;
updateMsgTxtFromDriver("");
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
pC_->debugFlow(functionName);
if (relative) {
realPosition = previous_position_ + position / MULT;
} else {
realPosition = position / MULT;
}
startTime = time(NULL);
status6Time = 0;
starting = 1;
sprintf(command, "Q15%d=%12.4f", axisNo_, realPosition);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
waitStart = 1;
next_poll = -1;
return status;
}
/*--------------------------------------------------------------------------------------------------------
Return *moving until the motor moved started moving. This enables the start
command to be sent separatly.
----------------------------------------------------------------------------------------------------------*/
asynStatus LiftAxis::poll(bool *moving) {
asynStatus status;
// Protect against excessive polling
if (time(NULL) < next_poll) {
return asynSuccess;
}
status = getAxisStatus(moving);
if (*moving == false && waitStart == 1) {
*moving = true;
setIntegerParam(pC_->motorStatusMoving_, true);
setIntegerParam(pC_->motorStatusDone_, false);
}
if (*moving) {
waitStart = 0;
}
if (*moving) {
next_poll = time(NULL) + BUSYPOLL;
} else {
next_poll = time(NULL) + IDLEPOLL;
}
callParamCallbacks();
return status;
}
/*--------------------------------------------------------------------------------------------------------------*/
asynStatus LiftAxis::stop(double acceleration) {
waitStart = 0;
return pmacAxis::stop(acceleration);
}
/*-------------------------------------------------------------------------------------------------------------*/
pmacV3Axis::pmacV3Axis(pmacController *pController, int axisNo)
: pmacAxis(pController, axisNo, false){
// read max speed
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
sprintf(command, "Q%2.2d04", axisNo_);
asynStatus cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
int nvals = sscanf(response, "%lf", &Speed);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read speed, "
"Status: %d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver("Cannot read Axis speed");
}
};
asynStatus pmacV3Axis::poll(bool *moving) {
asynStatus status = asynSuccess;
static const char *functionName = "pmacV3Axis::poll";
char message[132];
sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_);
pC_->debugFlow(message);
// Now poll axis status
if ((status = this->getAxisStatus(moving)) != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: getAxisStatus failed to return asynSuccess. Controller: %s, "
"Axis: %d.\n",
functionName, pC_->portName, axisNo_);
}
callParamCallbacks();
return status;
}
asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
char command[pC_->PMAC_MAXBUF_];
char response[pC_->PMAC_MAXBUF_];
asynStatus cmdStatus = asynSuccess;
int done = 0;
double position = 0;
int nvals = 0;
int axisProblemFlag = 0, axStat = 0;
int acknowledge = 0;
epicsUInt32 axErr = 0, highLim = 0, lowLim = 0;
char message[132], *axMessage;
__attribute__((unused)) static const char *functionName =
"pmacV3Axis::getAxisStatus";
sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%lf %d %d", &position, &axStat, &acknowledge);
if (cmdStatus || nvals != 3) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read position and status, "
"Status: %d\nCommand :%s\nParsed: %d\nResponse:%s\n",
cmdStatus, command, nvals, response);
updateMsgTxtFromDriver("Cannot read Axis position and status");
}
pmacV3Controller *p3C_ = (pmacV3Controller *)pC_;
IsEnable = axStat != -3;
setIntegerParam(p3C_->axisEnabled_, IsEnable);
asynStatus st = setIntegerParam(p3C_->enableAxis_, IsEnable);
cmdStatus = cmdStatus > st ? cmdStatus : st;
int direction = 0;
st = setDoubleParam(pC_->motorPosition_, position * MULT);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
cmdStatus = cmdStatus > st ? cmdStatus : st;
/* Use previous position and current position to calculate direction.*/
if ((position - previous_position_) > 0) {
direction = 1;
} else if (position - previous_position_ == 0.0) {
direction = previous_direction_;
} else {
direction = 0;
}
st = setIntegerParam(pC_->motorStatusDirection_, direction);
cmdStatus = cmdStatus > st ? cmdStatus : st;
/*Store position to calculate direction for next poll.*/
previous_position_ = position;
previous_direction_ = direction;
if (axStat <= 0 && starting == 0) {
done = 1;
} else {
starting = 0;
done = 0;
}
// /*
// code to test against too long status 5 or 6
// */
int EstimatedTimeOfArrival = 120;
if (axStat == 5 || axStat == 6) {
if (status6Time == 0) {
status6Time = time(NULL);
statusPos = position;
EstimatedTimeOfArrival =
std::ceil(2 * std::fabs(setpointPosition_ - position) / Speed);
} else {
if (time(NULL) > status6Time + EstimatedTimeOfArrival) {
/* trigger error only when not moving */
if (abs(position - statusPos) < .1) {
done = 1;
errlogPrintf(
"Axis %d stayed in 5,6 for more than %d seconds BROKEN\n",
axisNo_, EstimatedTimeOfArrival);
updateMsgTxtFromDriver("Axis stayed in 5,6 for more than estimated time: BROKEN");
status6Time = 0;
return cmdStatus;
} else {
status6Time = time(NULL);
statusPos = position;
}
}
}
}
if (!done) {
*moving = true;
st = setIntegerParam(pC_->motorStatusMoving_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusDone_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
} else {
*moving = false;
st = setIntegerParam(pC_->motorStatusMoving_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
st = setIntegerParam(pC_->motorStatusDone_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
if (homing) {
st = setIntegerParam(pC_->motorStatusHomed_, done);
cmdStatus = cmdStatus > st ? cmdStatus : st;
homing = 0;
}
}
if (*moving) {
return cmdStatus;
}
// read max speed
sprintf(command, "Q%2.2d04", axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%lf", &Speed);
if (cmdStatus || nvals != 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read speed, "
"Status: %d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver("Cannot read Axis speed");
}
sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
st = pC_->lowLevelWriteRead(axisNo_, command, response);
cmdStatus = cmdStatus > st ? cmdStatus : st;
nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
if (st || nvals != 3) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read axis Error, high and low limit "
"flags Status: %d\nCommand :%s\nResponse:%s\n",
st, command, response);
updateMsgTxtFromDriver(
"Cannot read Axis Error, high and low limit flag Status");
}
sprintf(message, "poll results: axis %d, status %d, axErr %d, done = %d",
axisNo_, axStat, axErr, done);
pC_->debugFlow(message);
/* search for trouble */
if (highLim) {
st = setIntegerParam(pC_->motorStatusHighLimit_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
updateMsgTxtFromDriver("High Limit Hit");
} else {
st = setIntegerParam(pC_->motorStatusHighLimit_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
}
if (lowLim) {
st = setIntegerParam(pC_->motorStatusLowLimit_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
updateMsgTxtFromDriver("Low Limit Hit");
} else {
st = setIntegerParam(pC_->motorStatusLowLimit_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
}
if (axErr == 11) {
st = setIntegerParam(pC_->motorStatusFollowingError_, true);
cmdStatus = cmdStatus > st ? cmdStatus : st;
updateMsgTxtFromDriver("Following error triggered");
} else {
st = setIntegerParam(pC_->motorStatusFollowingError_, false);
cmdStatus = cmdStatus > st ? cmdStatus : st;
}
/* Set any axis specific general problem bits. */
if (axErr != 0) {
axisProblemFlag = 1;
if (axisErrorCount < 10) {
axMessage = translateAxisError(axErr);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error "
"code %d, translated: %s:, status code = %d\n",
axisNo_, axErr, axMessage, axStat);
snprintf(message, sizeof(message), "PMAC Axis error: %s", axMessage);
updateMsgTxtFromDriver(message);
if (axMessage != NULL) {
free(axMessage);
}
axisErrorCount++;
} else if (axisErrorCount == 10) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Suppressing further axis error messages\n");
axisErrorCount++;
}
} else {
axisProblemFlag = 0;
axisErrorCount = 0;
}
// test for more error conditions encoded in axStat
switch(axStat) {
case -4:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Emergency stop activated", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
case -5:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Motor is not installed", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
case -3:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Axis disabled", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
case -2:
case -1:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Axis locked", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
}
st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
cmdStatus = cmdStatus > st ? cmdStatus : st;
return cmdStatus;
}
asynStatus pmacV3Axis::move(double position, int relative, double min_velocity,
double max_velocity, double acceleration) {
if(!IsEnable) {
updateMsgTxtFromDriver("Error: axis disabled");
return asynSuccess;
}
updateMsgTxtFromDriver("");
return pmacAxis::move(position, relative, min_velocity, max_velocity, acceleration);
}
/*========================= Amor Detector Motor code ==============*/
AmorDetectorAxis::AmorDetectorAxis(pmacController *pC, int axisNo, int function)
: pmacAxis(pC, axisNo, false) {
static const char *functionName = "pmacAxis::pmacAxis";
pC_->debugFlow(functionName);
_function = function;
callParamCallbacks();
/* Wake up the poller task which will make it do a poll,
* updating values for this axis to use the new resolution (stepSize_) */
pC_->wakeupPoller();
}
/*-----------------------------------------------------------------*/
asynStatus AmorDetectorAxis::moveVelocity(double min_velocity,
double max_velocity,
double acceleration)
{
updateMsgTxtFromDriver("Function moveVelocity not allowed");
return asynError;
}
/*------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::home(double min_velocity,
double max_velocity,
double acceleration,
int forwards)
{
updateMsgTxtFromDriver("Cannot home AMOR detector motors");
return asynError;
}
/*-------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::setPosition(double position)
{
updateMsgTxtFromDriver("Cannot setPosition() AMOR detector motors");
return asynError;
}
/*-------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::move(double position, int relative,
double min_velocity, double max_velocity,
double acceleration)
{
asynStatus status = asynError;
static const char *functionName = "AmorDetectorAxis::move";
double realPosition;
int parkStatus;
time_t errorWait;
updateMsgTxtFromDriver("");
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
pC_->debugFlow(functionName);
/* test if we are not in the change position */
sprintf(command,"P358");
pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "%d", &parkStatus);
if(_function != ADPARK && parkStatus == 2) {
updateMsgTxtFromDriver("Cannot run motor when in park position");
errlogPrintf("Cannot run motor when in park position");
return asynError;
}
if (relative) {
realPosition = previous_position_ + position / MULT;
} else {
realPosition = position / MULT;
}
/* set target */
errlogPrintf("function %d, sending position %f\n", _function, realPosition);
switch(_function){
case ADCOM:
sprintf(command, "Q451=%f", realPosition);
break;
case ADCOZ:
sprintf(command, "Q454=%f", realPosition);
break;
case ADPARK:
// test if a reset is required first
sprintf(command, "P359");
pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "%d", &parkStatus);
errlogPrintf("park status %d\n", parkStatus);
if(parkStatus != 0){
sprintf(command,"P352=2");
pC_->lowLevelWriteRead(axisNo_, command, response);
errlogPrintf("Park status %d, error reset: %s\n", parkStatus, command);
}
errorWait = time(NULL);
while(true){
usleep(5000);
sprintf(command, "P359");
pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "%d", &parkStatus);
errlogPrintf("P359 = %d\n", parkStatus);
if(parkStatus == 0){
break;
}
if(time(NULL) > errorWait + 3){
errlogPrintf("Failed to clear error status after 3 seconds\n");
updateMsgTxtFromDriver("Failed to clear errorState in 3 seconds");
return asynError;
}
}
// now drive to the real place
if(realPosition == 0) {
sprintf(command, "P350=3");
} else if(realPosition < -90){
sprintf(command, "P350=2");
} else {
updateMsgTxtFromDriver("Can only reach -100, 0, nothing else");
return asynError;
}
errlogPrintf("Park position %f, park command: %s\n", realPosition, command);
}
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::move failed to send command %s", command);
updateMsgTxtFromDriver("Cannot send Amor Detector Command");
return status;
}
/* send move command */
if(_function != ADPARK) {
sprintf(command, "P350=1");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::move failed to send command %s", command);
updateMsgTxtFromDriver("Cannot send Amor Detector Command");
}
}
det_starting = true;
det_startTime = time(NULL);
next_poll = BUSYPOLL;
return status;
}
/*-----------------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::stop(double acceleration) {
asynStatus status = asynError;
static const char *functionName = "AmorDetectorAxis::stopAxis";
bool moving = false;
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
this->poll(&moving);
det_starting = false;
if(moving) {
// only send a stop when actually moving
sprintf(command, "P350=8");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
}
return status;
}
/*-------------------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::poll(bool *moving)
{
asynStatus status = asynError;
static const char *functionName = "AmorDetectorAxis::poll";
int targetReached, errorCode, parkCode;
double position;
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
char errorMessage[128] = {0};
/* read in position flag*/
sprintf(command, "P354");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess){
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::poll failed to read in position");
updateMsgTxtFromDriver("Failed to read Amor Detector in position");
return status;
}
sscanf(response, "%d", &targetReached);
//if(_function == ADCOM){
// errlogPrintf("startTime: %ld, current time: %ld, starting flag %d, targetReached %d\n", det_startTime, time(NULL),
// det_starting, targetReached);
//}
// The thing is sometimes slow to start, allow 7 seconds
// Especially coz is really slow to indicate
if(det_starting && time(NULL) > (det_startTime + 7)) {
det_starting = false;
//if(_function == ADCOM){
// errlogPrintf("Resetting starting flag after timeout\n");
//}
}
setIntegerParam(pC_->motorStatusProblem_, false);
if(targetReached == 0 && det_starting == false){
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = IDLEPOLL;
} else {
*moving = true;
if(targetReached == 1){
det_starting = false;
//if(_function == ADCOM){
// errlogPrintf("Resetting starting flag following targetReached flag\n");
//}
}
setIntegerParam(pC_->motorStatusDone_, false);
}
// read position
if(_function == ADCOM) {
strcpy(command,"Q0510");
} else if(_function == ADCOZ) {
strcpy(command, "Q454");
} else if(_function == ADPARK){
strcpy(command,"P358");
}
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess){
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::poll failed to read value");
updateMsgTxtFromDriver("Failed to read Amor Detector value");
return status;
}
if(_function != ADPARK){
sscanf(response, "%lf", &position);
} else {
// analyse the park code to check where the thing is. This does
// not show good values while driving. But that is a limitation
// of the hardware.
sscanf(response, "%d", &parkCode);
if(parkCode == 1) {
position = 0;
} else {
position = -100;
}
// errlogPrintf("parkCode %d , position %f,targetREached = %d\n", parkCode, position, targetReached);
}
setDoubleParam(pC_->motorPosition_, position * 1000.);
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
//if(_function == ADCOM){
// errlogPrintf("polling for function %d, position %f, pos*10^3 %f, targetREached = %d\n", _function, position,
// position*MULT, targetReached);
// }
// read error status
sprintf(command, "P359");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status == asynSuccess) {
sscanf(response, "%d", &errorCode);
switch(errorCode) {
case 0:
break;
case 1:
strcpy(errorMessage,"COZ not released");
break;
case 2:
strcpy(errorMessage,"Motor is in wrong state");
break;
case 3:
strcpy(errorMessage,"FTZ error while moving");
break;
case 4:
strcpy(errorMessage, "Break while moving");
break;
case 5:
strcpy(errorMessage, "No freigabe during move");
break;
default:
strcpy(errorMessage, "Unknown error code from: ");
strcat(errorMessage, response);
break;
}
if(strlen(errorMessage) > 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::poll: %s", errorMessage);
updateMsgTxtFromDriver(errorMessage);
status = asynError;
}
}
callParamCallbacks();
return status;
}
/*-------------------------------------------------------------------------------*/
GirderAxis::GirderAxis(pmacController *pController, int axisNo)
: pmacV3Axis(pController, axisNo){
// GirderAxis expects a pmacV3Controller. Therefore it is checked whether the pointer pController can be cast
// to this object type. If this is not possible, the user made an error in the configuration files. This is documented
// in a corresponding error; after that, an exception is thrown to avoid returning an "illegal" instance of GirderAxis.
pmacV3Controller* pV3Controller = dynamic_cast<pmacV3Controller*>(pController);
if (pV3Controller == nullptr) {
errlogPrintf("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files.");
throw std::invalid_argument("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files.");
}
// Initial values for member variables
next_poll = -1;
previous_position_ = 0.0;
previous_direction_ = 0;
status6Time = 0;
statusPos = 0.0;
homing = 0;
axisErrorCount = 0;
};
/*-------------------------------------------------------------------------------*/
asynStatus GirderAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration) {
// If the axis is not enabled, do nothing
if(!IsEnable) {
updateMsgTxtFromDriver("Error: axis disabled");
return asynError;
}
// =======================================
// Local variable declaration
asynStatus status = asynError;
char command[pC_->PMAC_MAXBUF_] = {0};
char response[pC_->PMAC_MAXBUF_] = {0};
double realPosition = 0;
// =======================================
updateMsgTxtFromDriver("");
static const char *functionName = "GirderAxis::move";
pC_->debugFlow(functionName);
if (relative) {
realPosition = previous_position_ + position / MULT;
} else {
realPosition = position / MULT;
}
startTime = time(NULL);
status6Time = 0;
starting = 1;
// Set target position
snprintf(command, sizeof(command), "Q251=%f", realPosition);
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if (status == asynError) {
updateMsgTxtFromDriver("Error: Could not set target position");
return asynError;
}
// Start motion
snprintf(command, sizeof(command), "P150=1");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if (status == asynError) {
updateMsgTxtFromDriver("Error: Could not start motion");
return asynError;
}
next_poll = -1;
return status;
}
asynStatus GirderAxis::stop(double acceleration) {
// =======================================
// Local variable declaration
asynStatus status = asynSuccess;
static const char *functionName = "GirderAxis::stop";
bool moving = false;
char command[pC_->PMAC_MAXBUF_] = {0};
char response[pC_->PMAC_MAXBUF_] = {0};
// =======================================
pC_->debugFlow(functionName);
this->poll(&moving);
if(moving) {
// only send a stop when actually moving
snprintf(command, sizeof(command), "P150=8");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
}
return status;
}
asynStatus GirderAxis::poll(bool *moving) {
// =======================================
// Local variable declaration
asynStatus status = asynSuccess;
static const char *functionName = "GirderAxis::poll";
char command[pC_->PMAC_MAXBUF_] = {0};
char response[pC_->PMAC_MAXBUF_] = {0};
char message[132], *axMessage;
double position = 0;
int moving_to_position = 0, nvals = 0, axisProblemFlag = 0, axStat = 0, axError = 0, isEnabled = 1;
asynStatus st;
// =======================================
// Check for girder axis specific errors
snprintf(command, sizeof(command), "P159");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "P159=%d", &axError);
switch(axError) {
case 1:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Axis not switched on", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
isEnabled = 0;
break;
case 2:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Axis not ready for new command", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
case 3:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Axis 1 ERROR during motion", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
case 4:
snprintf(message, sizeof(message), "PMAC: %s on %d",
"Axis 2 ERROR during motion", axisNo_);
updateMsgTxtFromDriver(message);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s\n", message);
axisProblemFlag = 1;
break;
}
st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
status = status > st ? status : st;
// Downcast the pointer pmacController to pmacV3Controller in a typesafe manner
pmacV3Controller* p3C_ = dynamic_cast<pmacV3Controller*>(pC_);
if (p3C_ == nullptr) {
errlogPrintf("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files.");
throw std::invalid_argument("A GirderAxis instance needs a pmacV3Controller. Please check the configuration files.");
}
setIntegerParam(p3C_->axisEnabled_, isEnabled);
st = setIntegerParam(p3C_->enableAxis_, isEnabled);
status = status > st ? status : st;
int direction = 0;
/*
In GirderAxis::move, the user input is scaled by /MULT. Hence, the output of
Qxx10 needs to be scaled by *MULT
*/
st = setDoubleParam(pC_->motorPosition_, position * MULT);
status = status > st ? status : st;
st = setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
status = status > st ? status : st;
// Calculate the current (or last) movement direction
if ((position - previous_position_) > 0) {
direction = 1;
} else if (position - previous_position_ == 0.0) {
direction = previous_direction_;
} else {
direction = 0;
}
st = setIntegerParam(pC_->motorStatusDirection_, direction);
status = status > st ? status : st;
// Store the position which was read out from the hardware together with the calculated direction for the next poll
previous_position_ = position;
previous_direction_ = direction;
// Is the axis currently moving?
snprintf(command, sizeof(command), "P154");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "P154=%d", &moving_to_position);
/*
This code tests whether the axis is too long in status 5 or 6.
If the axis is in status 5 or 6, a time counter (status6Time) is started and updated during subsequent polls.
If status6Time exceeds the estimated arrival time of 120 seconds, a corresponding error is returned.
*/
int EstimatedTimeOfArrival = 120; // seconds
if (axStat == 5 || axStat == 6) {
if (status6Time == 0) {
status6Time = time(nullptr);
statusPos = position;
} else {
if (time(nullptr) > status6Time + EstimatedTimeOfArrival) {
/* trigger error only when not moving */
if (abs(position - statusPos) < .1) {
moving_to_position = 0;
errlogPrintf(
"Axis %d stayed in status 5 or 6 for more than %d seconds BROKEN\n",
axisNo_, EstimatedTimeOfArrival);
updateMsgTxtFromDriver("Axis stayed in status 5 or 6 for more than estimated time: BROKEN");
status6Time = 0;
return status;
} else {
status6Time = time(nullptr);
statusPos = position;
}
}
}
}
/*
If the axis is moving, set the corresponding flags in the pmacController (pC_) and end the poll.
*/
if (!moving_to_position) {
*moving = true;
st = setIntegerParam(pC_->motorStatusMoving_, true);
status = status > st ? status : st;
st = setIntegerParam(pC_->motorStatusDone_, false);
return status > st ? status : st;
} else {
*moving = false;
st = setIntegerParam(pC_->motorStatusMoving_, false);
status = status > st ? status : st;
st = setIntegerParam(pC_->motorStatusDone_, true);
status = status > st ? status : st;
}
/* Set any axis specific general problem bits. */
if (axError != 0) {
axisProblemFlag = 1;
if (axisErrorCount < 10) {
axMessage = translateAxisError(axError);
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvGirderAxisGetStatus: Axis %d is in deep trouble: axis error "
"code %d, translated: %s:, status code = %d\n",
axisNo_, axError, axMessage, axStat);
snprintf(message, sizeof(message), "GirderAxis error: %s", axMessage);
updateMsgTxtFromDriver(message);
if (axMessage != NULL) {
free(axMessage);
}
axisErrorCount++;
} else if (axisErrorCount == 10) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"Suppressing further axis error messages\n");
axisErrorCount++;
}
} else {
axisProblemFlag = 0;
axisErrorCount = 0;
}
st = setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
status = status > st ? status : st;
return status;
}
Reference in New Issue View Git Blame Copy Permalink