Initial commit of SINQ EPICS Application
This commit is contained in:
505
sinqEPICSApp/src/pmacAxis.cpp
Normal file
505
sinqEPICSApp/src/pmacAxis.cpp
Normal file
@@ -0,0 +1,505 @@
|
||||
/********************************************
|
||||
* 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 PMACS is that they can be programmed.
|
||||
* This affects also the commands they understand.
|
||||
* Our PMACS 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
|
||||
********************************************/
|
||||
|
||||
#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"
|
||||
|
||||
|
||||
#define MULT 1000.
|
||||
|
||||
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)
|
||||
: asynMotorAxis(pC, axisNo),
|
||||
pC_(pC)
|
||||
{
|
||||
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 (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(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(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(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);
|
||||
|
||||
setDoubleParam(pC_->motorLowLimit_, low_limit);
|
||||
setDoubleParam(pC_->motorHighLimit_, high_limit);
|
||||
setIntegerParam(pC_->motorStatusHasEncoder_, 1);
|
||||
|
||||
// Enable the axis. After startup, the axis are disabled on the controller...
|
||||
sprintf(command, "M%2.2d14=1", axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(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;
|
||||
|
||||
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(command, response);
|
||||
|
||||
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);
|
||||
|
||||
sprintf(command, "M%2.2d=9", axisNo_);
|
||||
|
||||
status = pC_->lowLevelWriteRead(command, response);
|
||||
homing = 1;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus pmacAxis::moveVelocity(double min_velocity, double max_velocity, double acceleration)
|
||||
{
|
||||
asynStatus status = asynError;
|
||||
char command[128] = {0};
|
||||
char response[32] = {0};
|
||||
static const char *functionName = "pmacAxis::moveVelocity";
|
||||
|
||||
pC_->debugFlow(functionName);
|
||||
|
||||
return asynError; // can we do this, actually?
|
||||
|
||||
// status = pC_->lowLevelWriteRead(command, response);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
asynStatus pmacAxis::setPosition(double position)
|
||||
{
|
||||
//int status = 0;
|
||||
static const char *functionName = "pmacAxis::setPosition";
|
||||
|
||||
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(command, response);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus pmacAxis::poll(bool *moving)
|
||||
{
|
||||
int status = 0;
|
||||
static const char *functionName = "pmacAxis::poll";
|
||||
char message[132];
|
||||
|
||||
sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_);
|
||||
pC_->debugFlow(message);
|
||||
|
||||
//Now poll axis status
|
||||
if ((status = 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 ? asynError : asynSuccess;
|
||||
}
|
||||
|
||||
static char *translateAxisError(int axErr)
|
||||
{
|
||||
switch(axErr){
|
||||
case 0:
|
||||
return strdup("no error");
|
||||
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;
|
||||
double position = 0;
|
||||
double enc_position = 0;
|
||||
int nvals = 0;
|
||||
int axisProblemFlag = 0;
|
||||
int limitsDisabledBit = 0;
|
||||
epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim= 0;
|
||||
int errorPrintMin = 10000;
|
||||
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(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 );
|
||||
}
|
||||
|
||||
sprintf(command,"Q%2.2d10",axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(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 );
|
||||
}
|
||||
|
||||
sprintf(command,"P%2.2d00",axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(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 );
|
||||
}
|
||||
|
||||
sprintf(command,"M%2.2d21", axisNo_);
|
||||
cmdStatus = pC_->lowLevelWriteRead(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(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;
|
||||
|
||||
/* 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_);
|
||||
starting = 0;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
code to check for checking against too long status 6
|
||||
*/
|
||||
if(axStat == 5 || axStat == 6){
|
||||
if(status6Time == 0){
|
||||
status6Time = time(NULL);
|
||||
} else {
|
||||
if(time(NULL) > status6Time + 60){
|
||||
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_);
|
||||
status6Time = 0;
|
||||
return asynSuccess;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 );
|
||||
} else {
|
||||
setIntegerParam(pC_->motorStatusHighLimit_, false );
|
||||
}
|
||||
if(lowLim){
|
||||
setIntegerParam(pC_->motorStatusLowLimit_, true );
|
||||
} else {
|
||||
setIntegerParam(pC_->motorStatusLowLimit_, false );
|
||||
}
|
||||
if(axErr == 11) {
|
||||
setIntegerParam(pC_->motorStatusFollowingError_,true );
|
||||
} 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);
|
||||
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
Block a user