506 lines
14 KiB
C++
506 lines
14 KiB
C++
/********************************************
|
|
* 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;
|
|
}
|
|
|