diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 6d1b89a..5931807 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -1,54 +1,53 @@ /******************************************** * pmacAxis.cpp - * - * PMAC Asyn motor based on the + * + * PMAC Asyn motor based on the * asynMotorAxis class. - * + * * Matthew Pearson * 23 May 2012 - * - * Substantially modified for use at SINQ at PSI. + * + * 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. + * 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. + * 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 + * 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 -#include -#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include +#include #include "pmacController.h" - -#include -template -const char* getClassName(T) { +template +const char *getClassName(T) { return typeid(T).name(); } @@ -59,8 +58,7 @@ const char* getClassName(T) { #define ABS(x) (x < 0 ? -(x) : (x)) -extern "C" void shutdownCallback(void *pPvt) -{ +extern "C" void shutdownCallback(void *pPvt) { pmacController *pC = static_cast(pPvt); pC->lock(); @@ -70,15 +68,12 @@ extern "C" void shutdownCallback(void *pPvt) // These are the pmacAxis class methods pmacAxis::pmacAxis(pmacController *pC, int axisNo, bool enable) - : SINQAxis(pC, axisNo), - pC_(pC), - autoEnable(enable) -{ + : SINQAxis(pC, axisNo), pC_(pC), autoEnable(enable) { static const char *functionName = "pmacAxis::pmacAxis"; - pC_->debugFlow(functionName); + pC_->debugFlow(functionName); - //Initialize non-static data members + // Initialize non-static data members setpointPosition_ = 0.0; encoderPosition_ = 0.0; currentVelocity_ = 0.0; @@ -96,26 +91,26 @@ pmacAxis::pmacAxis(pmacController *pC, int axisNo, bool enable) homing = 0; next_poll = -1; - /* Set an EPICS exit handler that will shut down polling before asyn kills the IP sockets */ + /* 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 + // 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_); + "%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_) */ + /* 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) -{ +asynStatus pmacAxis::getAxisInitialStatus(void) { char command[pC_->PMAC_MAXBUF_]; char response[pC_->PMAC_MAXBUF_]; int cmdStatus = 0; @@ -127,46 +122,53 @@ asynStatus pmacAxis::getAxisInitialStatus(void) static const char *functionName = "pmacAxis::getAxisInitialStatus"; sprintf(command, "Q%2.2d00", axisNo_); - cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + 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_); + 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); + 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_); + 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_; + } + high_limit = ((double)limVal) * scale_; sprintf(command, "I%d14", axisNo_); - cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + 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_); + 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_; + } + low_limit = ((double)limVal) * scale_; char message[132]; - sprintf(message,"scale_factor = %lf, high = %lf, low = %lf", scale_,high_limit, low_limit); + 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. + 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); + if (high_limit > low_limit) { + setDoubleParam(pC_->motorLowLimit_, low_limit); setDoubleParam(pC_->motorHighLimit_, high_limit); } else { - setDoubleParam(pC_->motorLowLimit_, high_limit); + setDoubleParam(pC_->motorLowLimit_, high_limit); setDoubleParam(pC_->motorHighLimit_, low_limit); } @@ -174,75 +176,74 @@ asynStatus pmacAxis::getAxisInitialStatus(void) // 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_); + 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 +pmacAxis::~pmacAxis() { + // Destructor } - -asynStatus pmacAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration) -{ +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); + pC_->debugFlow(functionName); char command[128] = {0}; char response[32] = {0}; - pC_->debugFlow(functionName); + pC_->debugFlow(functionName); - if(relative){ - realPosition = previous_position_ + position/MULT; + if (relative) { + realPosition = previous_position_ + position / MULT; } else { - realPosition = position/MULT; + 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); + 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 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); + pC_->debugFlow(functionName); updateMsgTxtFromDriver(""); sprintf(command, "M%2.2d=9", axisNo_); - - status = pC_->lowLevelWriteRead(axisNo_,command, response); + + status = pC_->lowLevelWriteRead(axisNo_, command, response); homing = 1; next_poll = time(NULL) + BUSYPOLL; @@ -250,359 +251,25 @@ asynStatus pmacAxis::home(double min_velocity, double max_velocity, double accel return status; } -asynStatus pmacAxis::moveVelocity(double min_velocity, double max_velocity, double acceleration) -{ +asynStatus pmacAxis::moveVelocity(double min_velocity, double max_velocity, + double acceleration) { asynStatus status = asynError; static const char *functionName = "pmacAxis::moveVelocity"; - pC_->debugFlow(functionName); + pC_->debugFlow(functionName); - return asynError; // can we do this, actually? + return asynError; // can we do this, actually? // status = pC_->lowLevelWriteRead(axisNo_,command, response); return status; } - -asynStatus pmacAxis::setPosition(double position) -{ - //int status = 0; +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. @@ -610,337 +277,177 @@ asynStatus pmacAxis::enable(int on) { 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 pmacAxis::stop(double acceleration) { asynStatus status = asynError; - static const char *functionName = "SeleneAxis::home"; + static const char *functionName = "pmacAxis::stopAxis"; - 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); + pC_->debugFlow(functionName); char command[128] = {0}; char response[32] = {0}; - pC_->debugFlow(functionName); + sprintf(command, "M%2.2d=8", axisNo_); - 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); - + 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) -{ +asynStatus pmacAxis::poll(bool *moving) { int status = 0; - static const char *functionName = "pmacV3Axis::poll"; + static const char *functionName = "pmacAxis::poll"; char message[132]; // Protect against excessive polling - if(time(NULL) < next_poll){ + if (time(NULL) < next_poll) { return asynSuccess; } sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_); - pC_->debugFlow(message); + pC_->debugFlow(message); - //Now poll axis status + // 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_); + "%s: getAxisStatus failed to return asynSuccess. Controller: %s, " + "Axis: %d.\n", + functionName, pC_->portName, axisNo_); } - if(*moving){ + 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 pmacV3Axis::getAxisStatus(bool *moving) { +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; - epicsUInt32 axErr = 0, highLim = 0, lowLim = 0, axDone = 0; - int axStat=0; + int axisProblemFlag = 0, axStat = 0; + epicsUInt32 axErr = 0, highLim = 0, lowLim = 0; char message[132], *axMessage; - static const char *functionName = "pmacV3Axis::getAxisStatus"; - - pmacV3Controller* p3C_ = (pmacV3Controller*)pC_; - sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_); + /* 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, "%lf %d %d", &position, &axStat, &axDone); - if (cmdStatus || nvals != 3) { + nvals = sscanf(response, "%d", &axErr); + if (cmdStatus || nvals != 1) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvPmacAxisGetStatus: Failed to read position and status, " - "Status: %d\nCommand :%s\nResponse:%s\n", + "drvPmacAxisGetStatus: Failed to read axis Error Status: " + "%d\nCommand :%s\nResponse:%s\n", cmdStatus, command, response); - updateMsgTxtFromDriver("Cannot read Axis position and status"); + updateMsgTxtFromDriver("Cannot read Axis Error Status"); } - - int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0); - // st = callParamCallbacks(); - // printf("axStat: callParamCallbacks -> ok: %d\n", st == asynSuccess); + 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.*/ - int direction = previous_direction_; if ((position - previous_position_) > 0) { direction = 1; - } - if ((position - previous_position_) < 0) { + } else if (position - previous_position_ == 0.0) { + direction = previous_direction_; + } else { direction = 0; } setIntegerParam(pC_->motorStatusDirection_, direction); @@ -949,10 +456,11 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { previous_position_ = position; previous_direction_ = direction; - // errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position); + // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, + // axErr, position); /* are we done? */ - if ( axDone == 0 && starting == 0) { + if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) { done = 1; } else { starting = 0; @@ -971,7 +479,6 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_); updateMsgTxtFromDriver("Axis did not start within 10 seconds"); starting = 0; - callParamCallbacks(); return asynSuccess; } @@ -997,7 +504,6 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { updateMsgTxtFromDriver( "Axis stayed in 5,6 for more then 60 seconds: BROKEN"); status6Time = 0; - callParamCallbacks(); return asynSuccess; } else { status6Time = time(NULL); @@ -1021,58 +527,34 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { } } - sprintf(message, "poll results: axis %d, status %d, done = %d", - axisNo_, axStat, axDone); + sprintf(message, "poll results: axis %d, status %d, axErr %d, done = %d", + axisNo_, axStat, axErr, done); 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. */ + /* search for trouble */ + if (highLim) { + setIntegerParam(pC_->motorStatusHighLimit_, true); + updateMsgTxtFromDriver("High Limit Hit"); + } else { + setIntegerParam(pC_->motorStatusHighLimit_, false); } - - if ((axStat < 0 && axStat != -3) || axErr != 0) { + 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); - printf("axErr: %d\taxStat: %d\taxMessage: %s\n", axErr, axStat, axMessage); asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error " "code %d, translated: %s:, status code = %d\n", @@ -1094,8 +576,713 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { } setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag); - callParamCallbacks(); 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, highLim = 0, lowLim = 0, axDone = 0; +// int axStat=0; +// char message[132], *axMessage; + +// static const char *functionName = "pmacV3Axis::getAxisStatus"; + +// pmacV3Controller* p3C_ = (pmacV3Controller*)pC_; +// 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"); +// } + +// int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0); +// // st = callParamCallbacks(); +// // printf("axStat: callParamCallbacks -> ok: %d\n", st == asynSuccess); + +// 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; + +// // 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; +// callParamCallbacks(); +// 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; +// callParamCallbacks(); +// 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 && axStat != -3) || axErr != 0) { +// axisProblemFlag = 1; +// if (axisErrorCount < 10) { +// axMessage = translateAxisError(axErr); +// printf("axErr: %d\taxStat: %d\taxMessage: %s\n", axErr, axStat, +// axMessage); 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); + +// callParamCallbacks(); +// return 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, axStat = 0, axDone; + epicsUInt32 axErr = 0, highLim = 0, lowLim = 0; + char message[132], *axMessage; + + 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, &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"); + } + + pmacV3Controller *p3C_ = (pmacV3Controller *)pC_; + int st = setIntegerParam(p3C_->axisEnabled_, axStat >= 0); + + 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 != 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", + cmdStatus, command, response); + updateMsgTxtFromDriver( + "Cannot read Axis Error, high and low limit flag Status"); + } + + 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; +}