lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
fbf2331a05499059607b9b17d7470d3b6a86acd8
sinqepicsapp/sinqEPICSApp/src/pmacAxis.cpp
brambilla_m fbf2331a05 pmacV3 read axis state
2022-05-20 08:35:42 +00:00

1091 lines
32 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
*
********************************************/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <epicsTime.h>
#include <epicsThread.h>
#include <epicsExport.h>
#include <epicsExit.h>
#include <epicsString.h>
#include <iocsh.h>
#include <errlog.h>
#include "pmacController.h"
#include <typeinfo>
template<typename T>
const char* getClassName(T) {
return typeid(T).name();
}
#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: enaabling 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;
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 = asynError;
static const char *functionName = "pmacAxis::stopAxis";
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
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 triggere");
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);
setIntegerParam(pC_->motorStatusProblem_, 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 = pmacAxis::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);
setIntegerParam(pC_->motorStatusProblem_, 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 = "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;
/* 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) { };
asynStatus pmacV3Axis::poll(bool *moving)
{
int status = 0;
static const char *functionName = "pmacV3Axis::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;
}
asynStatus pmacV3Axis::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;
epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0, axDone = 0;
char message[132], *axMessage;
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, &axDone);
if (cmdStatus || nvals != 3) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read position and status, "
"Status: %d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver("Cannot read Axis position and status");
}
setDoubleParam(pC_->motorPosition_, position * MULT);
setDoubleParam(pC_->motorEncoderPosition_, position * MULT);
/* Use previous position and current position to calculate direction.*/
int direction = previous_direction_;
if ((position - previous_position_) > 0) {
direction = 1;
}
if ((position - previous_position_) < 0) {
direction = 0;
}
setIntegerParam(pC_->motorStatusDirection_, direction);
/*Store position to calculate direction for next poll.*/
previous_position_ = position;
previous_direction_ = direction;
setIntegerParam(pC_->axisStatus_, axStat);
// errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position);
/* are we done? */
if ( axDone == 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);
setIntegerParam(pC_->motorStatusProblem_, 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, done = %d",
axisNo_, axStat, axDone);
pC_->debugFlow(message);
if (*moving == false) {
sprintf(command, "P%2.2d01 P%2.2d37 P%2.2d38", axisNo_, axisNo_, axisNo_);
cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim);
if (cmdStatus || nvals != 1) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvPmacAxisGetStatus: Failed to read axis Error, high and low limit "
"flags Status: %d\nCommand :%s\nResponse:%s\n",
cmdStatus, command, response);
updateMsgTxtFromDriver(
"Cannot read Axis Error, high and low limit flag Status");
}
// errlogPrintf("Polling, axErr = %d, highLim = %d, lowLim = %d\n", axStat,
// axErr, highLim, lowLim);
sprintf(message, "poll results: axis %d, axErr %d", axisNo_, axErr);
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 (axStat < 0 || 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;
}
Reference in New Issue View Git Blame Copy Permalink