#include "masterMacsAxis.h" #include "asynOctetSyncIO.h" #include "epicsExport.h" #include "iocsh.h" #include "masterMacsController.h" #include #include #include #include #include #include #include struct masterMacsAxisImpl { /* The axis status and axis error of MasterMACS are given as an integer from the controller. The 16 individual bits contain the actual information. */ std::bitset<16> axisStatus; std::bitset<16> axisError; bool waitForHandshake; time_t timeAtHandshake; bool needInit = true; bool targetReachedUninitialized; }; /* A special communication timeout is used in the following two cases: 1) Enable command 2) First move command after enabling the motor This is due to MasterMACS running a powerup cycle, which can delay the answer. */ #define PowerCycleTimeout 10.0 // Value in seconds /* Contains all instances of turboPmacAxis which have been created and is used in the initialization hook function. */ static std::vector axes; /** * @brief Hook function to perform certain actions during the IOC initialization * * @param iState */ static void epicsInithookFunction(initHookState iState) { if (iState == initHookAfterDatabaseRunning) { // Iterate through all axes of each and call the initialization method // on each one of them. for (std::vector::iterator itA = axes.begin(); itA != axes.end(); ++itA) { masterMacsAxis *axis = *itA; axis->init(); } } } void appendErrorMessage(char *fullMessage, size_t capacityFullMessage, const char *toBeAppended) { size_t lenFullMessage = strlen(fullMessage); size_t lenToBeAppended = strlen(toBeAppended); if (lenFullMessage == 0) { // The error message is empty -> Just copy the content of toBeAppended // into fullMessage, if the formers capacity suffices if (lenToBeAppended < capacityFullMessage) { // We check before that the capacity of fullMessage is sufficient strcpy(fullMessage, toBeAppended); } } else { // Append the message and add a linebreak in between, if the capacity of // fullMessage suffices. We need capacity for one additional character // because of the linebreak. if (lenFullMessage + lenToBeAppended + 1 < capacityFullMessage) { // Append the linebreak and readd the null terminator behind it // fullMessage[lenFullMessage] = '\n'; // fullMessage[lenFullMessage + 1] = '\0'; // We check before that the capacity of fullMessage is sufficient strcat(fullMessage, toBeAppended); } } } masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo) : sinqAxis(pC, axisNo), pC_(pC) { asynStatus status = asynSuccess; /* The superclass constructor sinqAxis calls in turn its superclass constructor asynMotorAxis. In the latter, a pointer to the constructed object this is stored inside the array pAxes_: pC->pAxes_[axisNo] = this; Therefore, the axes are managed by the controller pC. If axisNo is out of bounds, asynMotorAxis prints an error. However, we want the IOC creation to stop completely, since this is a configuration error. */ if (axisNo >= pC->numAxes()) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:: FATAL ERROR: " "Axis index %d must be smaller than the total number of axes " "%d. Call the support.", pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__, axisNo_, pC->numAxes()); exit(-1); } // Register the hook function during construction of the first axis object if (axes.empty()) { initHookRegister(&epicsInithookFunction); } // Collect all axes into this list which will be used in the hook function axes.push_back(this); pMasterMacsA_ = std::make_unique((masterMacsAxisImpl){ .axisStatus = std::bitset<16>(0), .axisError = std::bitset<16>(0), .waitForHandshake = false, .timeAtHandshake = 0, .targetReachedUninitialized = true, }); // masterMacs motors can always be disabled status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable(), 1); if (status != asynSuccess) { asynPrint( pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR " "(setting a parameter value failed with %s)\n. Terminating IOC", pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); exit(-1); } // Assume that the motor is initially not moving status = pC_->setIntegerParam(axisNo_, pC_->motorStatusMoving(), false); if (status != asynSuccess) { asynPrint( pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR " "(setting a parameter value failed with %s)\n. Terminating IOC", pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); } // Even though this happens already in sinqAxis, a default value for // motorMessageText is set here again, because apparently the sinqAxis // constructor is not run before the string is accessed? status = setStringParam(pC_->motorMessageText(), ""); if (status != asynSuccess) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR " "(setting a parameter value failed " "with %s)\n. Terminating IOC", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(status)); exit(-1); } } masterMacsAxis::~masterMacsAxis(void) { // Since the controller memory is managed somewhere else, we don't need to // clean up the pointer pC here. } /** Read out the following values: - current position - current velocity - maximum velocity - acceleration - Software limits */ asynStatus masterMacsAxis::init() { // Local variable declaration asynStatus pl_status = asynSuccess; char response[pC_->MAXBUF_] = {0}; int nvals = 0; double motorRecResolution = 0.0; double motorPosition = 0.0; double motorVelocity = 0.0; double motorVmax = 0.0; double motorAccel = 0.0; // ========================================================================= // The parameter library takes some time to be initialized. Therefore we // wait until the status is not asynParamUndefined anymore. time_t now = time(NULL); time_t maxInitTime = 60; while (1) { pl_status = pC_->getDoubleParam(axisNo_, pC_->motorRecResolution(), &motorRecResolution); if (pl_status == asynParamUndefined) { if (now + maxInitTime < time(NULL)) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " "%d\nInitializing the parameter library failed.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); return asynError; } } else if (pl_status == asynSuccess) { break; } else if (pl_status != asynSuccess) { return pC_->paramLibAccessFailed(pl_status, "motorRecResolution_", axisNo_, __PRETTY_FUNCTION__, __LINE__); } } // Read out the current position pl_status = pC_->read(axisNo_, 12, response); if (pl_status != asynSuccess) { return pl_status; } nvals = sscanf(response, "%lf", &motorPosition); if (nvals != 1) { return pC_->couldNotParseResponse("R12", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } // Read out the current velocity pl_status = pC_->read(axisNo_, 05, response); if (pl_status != asynSuccess) { return pl_status; } nvals = sscanf(response, "%lf", &motorVelocity); if (nvals != 1) { return pC_->couldNotParseResponse("R05", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } // Read out the maximum velocity pl_status = pC_->read(axisNo_, 26, response); if (pl_status != asynSuccess) { return pl_status; } nvals = sscanf(response, "%lf", &motorVmax); if (nvals != 1) { return pC_->couldNotParseResponse("R26", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } // Read out the acceleration pl_status = pC_->read(axisNo_, 06, response); if (pl_status != asynSuccess) { return pl_status; } nvals = sscanf(response, "%lf", &motorAccel); if (nvals != 1) { return pC_->couldNotParseResponse("R06", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } // Store the motor position in the parameter library pl_status = setMotorPosition(motorPosition); if (pl_status != asynSuccess) { return pl_status; } // Write to the motor record fields pl_status = setVeloFields(motorVelocity, 0.0, motorVmax); if (pl_status != asynSuccess) { return pl_status; } pl_status = setAcclField(motorAccel); if (pl_status != asynSuccess) { return pl_status; } pl_status = readEncoderType(); if (pl_status != asynSuccess) { return pl_status; } // Update the parameter library immediately pl_status = callParamCallbacks(); if (pl_status != asynSuccess) { // If we can't communicate with the parameter library, it doesn't // make sense to try and upstream this to the user -> Just log the // error asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " "%d:\ncallParamCallbacks failed with %s.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, pC_->stringifyAsynStatus(pl_status)); return pl_status; } // Axis is fully initialized setNeedInit(false); return pl_status; } // Perform the actual poll asynStatus masterMacsAxis::doPoll(bool *moving) { // Return value for the poll asynStatus poll_status = asynSuccess; // Status of read-write-operations of ASCII commands to the controller asynStatus rw_status = asynSuccess; // Status of parameter library operations asynStatus pl_status = asynSuccess; char response[pC_->MAXBUF_] = {0}; int nvals = 0; int direction = 0; bool hasError = false; double currentPosition = 0.0; double previousPosition = 0.0; double motorRecResolution = 0.0; double highLimit = 0.0; double lowLimit = 0.0; double limitsOffset = 0.0; double handshakePerformed = 0; // ========================================================================= // Does the axis need to be intialized? if (needInit()) { rw_status = init(); if (rw_status != asynSuccess) { return rw_status; } } // Are we currently waiting for a handshake? if (pMasterMacsA_->waitForHandshake) { // Check if the handshake takes too long and communicate an error in // this case. A handshake should not take more than 5 seconds. time_t currentTime = time(NULL); bool timedOut = (currentTime > pMasterMacsA_->timeAtHandshake + 5); if (pC_->getMsgPrintControl().shouldBePrinted( pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, timedOut, pC_->pasynUser())) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nAsked for a " "handshake at %ld s and didn't get a positive reply yet " "(current time is %ld s).\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, pMasterMacsA_->timeAtHandshake, currentTime); } if (timedOut) { setAxisParamChecked(this, motorMessageText, "Timed out while waiting for a handshake"); } pC_->read(axisNo_, 86, response); if (rw_status != asynSuccess) { return rw_status; } nvals = sscanf(response, "%lf", &handshakePerformed); if (nvals != 1) { return pC_->couldNotParseResponse("R86", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } if (handshakePerformed == 1.0) { // Handshake has been performed successfully -> Continue with the // poll pMasterMacsA_->waitForHandshake = false; pMasterMacsA_->targetReachedUninitialized = false; } else { // Still waiting for the handshake - try again in the next busy // poll. This is already part of the movement procedure. *moving = true; setAxisParamChecked(this, motorStatusMoving, *moving); setAxisParamChecked(this, motorStatusDone, !(*moving)); return asynSuccess; } } // Motor resolution from parameter library getAxisParamChecked(this, motorRecResolution, &motorRecResolution); // Read the previous motor position pl_status = motorPosition(&previousPosition); if (pl_status != asynSuccess) { return pl_status; } // Update the axis status rw_status = readAxisStatus(); if (rw_status != asynSuccess) { return rw_status; } if (pMasterMacsA_->targetReachedUninitialized) { *moving = false; } else { if (targetReached() || !switchedOn()) { *moving = false; } else { *moving = true; } } if (targetReached()) { pMasterMacsA_->targetReachedUninitialized = false; } // Read the current position rw_status = pC_->read(axisNo_, 12, response); if (rw_status != asynSuccess) { return rw_status; } nvals = sscanf(response, "%lf", ¤tPosition); if (nvals != 1) { return pC_->couldNotParseResponse("R12", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } /* Read out the error if either a fault condition status flag has been set or if a movement has just ended. */ msgPrintControlKey keyError = msgPrintControlKey( pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); if (faultConditionSet() || !(*moving)) { rw_status = readAxisError(); /* A communication error is a special case. If a communication between controller and axis error occurred, all subsequent errors are ignored, since this information is not reliable. */ if (communicationError()) { if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true, pC_->pasynUser())) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " "%d\nCommunication error.%s\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, pC_->getMsgPrintControl().getSuffix()); } setAxisParamChecked(this, motorMessageText, "Communication error between PC and motor " "controller. Please call the support."); poll_status = asynError; } else { // This buffer must be initialized to zero because we build the // error message by appending strings. char errorMessage[pC_->MAXBUF_] = {0}; char shellMessage[pC_->MAXBUF_] = {0}; // Concatenate all other errors if (shortCircuit()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Short circuit fault."); appendErrorMessage( errorMessage, sizeof(errorMessage), "Short circuit error. Please call the support."); poll_status = asynError; } if (encoderError()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Encoder error."); appendErrorMessage(errorMessage, sizeof(errorMessage), "Encoder error. Please call the support."); poll_status = asynError; } if (followingError()) { appendErrorMessage( shellMessage, sizeof(shellMessage), "Maximum callowed following error exceeded."); appendErrorMessage( errorMessage, sizeof(errorMessage), "Maximum allowed following error exceeded.Check if " "movement range is blocked. Otherwise please call the " "support."); poll_status = asynError; } if (feedbackError()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Feedback error."); appendErrorMessage(errorMessage, sizeof(errorMessage), "Feedback error. Please call the support."); poll_status = asynError; } /* Either the software limits or the end switches of the controller have been hit. Since the EPICS limits are derived from the software limits and are a little bit smaller, these error cases can only happen if either the axis has an incremental encoder which is not properly homed or if a bug occured. */ if (positiveLimitSwitch() || negativeLimitSwitch() || positiveSoftwareLimit() || negativeSoftwareLimit()) { // Distinction for developers if (positiveLimitSwitch()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Positive limit switch."); } if (negativeLimitSwitch()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Negative limit switch."); } if (positiveSoftwareLimit()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Positive software limit."); } if (negativeSoftwareLimit()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Negative software limit."); } // Generic error message for user appendErrorMessage( errorMessage, sizeof(errorMessage), "Software limits or end switch hit. Try homing the motor, " "moving in the opposite direction or check the SPS for " "errors (if available). Otherwise please call the " "support."); poll_status = asynError; } if (overCurrent()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Overcurrent error."); appendErrorMessage( errorMessage, sizeof(errorMessage), "Overcurrent error. Please call the support."); poll_status = asynError; } if (overTemperature()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Overtemperature error."); appendErrorMessage( errorMessage, sizeof(errorMessage), "Overtemperature error. Please call the support."); poll_status = asynError; } if (overVoltage()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Overvoltage error."); appendErrorMessage( errorMessage, sizeof(errorMessage), "Overvoltage error. Please call the support."); poll_status = asynError; } if (underVoltage()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "Undervoltage error."); appendErrorMessage( errorMessage, sizeof(errorMessage), "Undervoltage error. Please call the support."); poll_status = asynError; } if (stoFault()) { appendErrorMessage(shellMessage, sizeof(shellMessage), "STO input is on disable state."); appendErrorMessage(errorMessage, sizeof(errorMessage), "STO fault. Please call the support."); poll_status = asynError; } if (strlen(shellMessage) > 0) { if (pC_->getMsgPrintControl().shouldBePrinted( keyError, true, pC_->pasynUser())) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line " "%d\n%s%s\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, shellMessage, pC_->getMsgPrintControl().getSuffix()); } } setAxisParamChecked(this, motorMessageText, errorMessage); } } else { pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser()); } // Read out the limits, if the motor is not moving if (!(*moving)) { rw_status = pC_->read(axisNo_, 34, response); if (rw_status != asynSuccess) { return rw_status; } nvals = sscanf(response, "%lf", &lowLimit); if (nvals != 1) { return pC_->couldNotParseResponse("R34", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } rw_status = pC_->read(axisNo_, 33, response); if (rw_status != asynSuccess) { return rw_status; } nvals = sscanf(response, "%lf", &highLimit); if (nvals != 1) { return pC_->couldNotParseResponse("R33", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } /* The axis limits are set as: ({[]}) where [] are the positive and negative limits set in EPICS/NICOS, {} are the software limits set on the MCU and () are the hardware limit switches. In other words, the EPICS/NICOS limits must be stricter than the software limits on the MCU which in turn should be stricter than the hardware limit switches. For example, if the hardware limit switches are at [-10, 10], the software limits could be at [-9, 9] and the EPICS / NICOS limits could be at [-8, 8]. Therefore, we cannot use the software limits read from the MCU directly, but need to shrink them a bit. In this case, we're shrinking them by 0.1 mm or 0.1 degree (depending on the axis type) on both sides. */ getAxisParamChecked(this, motorLimitsOffset, &limitsOffset); highLimit = highLimit - limitsOffset; lowLimit = lowLimit + limitsOffset; setAxisParamChecked(this, motorHighLimitFromDriver, highLimit); setAxisParamChecked(this, motorLowLimitFromDriver, lowLimit); } // Update the enable PV setAxisParamChecked(this, motorEnableRBV, readyToBeSwitchedOn() && switchedOn()); if (*moving) { // If the axis is moving, evaluate the movement direction if ((currentPosition - previousPosition) > 0) { direction = 1; } else { direction = 0; } } // Update the parameter library if (hasError) { setAxisParamChecked(this, motorStatusProblem, true); } setAxisParamChecked(this, motorStatusMoving, *moving); setAxisParamChecked(this, motorStatusDone, !(*moving)); setAxisParamChecked(this, motorStatusDirection, direction); pl_status = setMotorPosition(currentPosition); if (pl_status != asynSuccess) { return pl_status; } return poll_status; } asynStatus masterMacsAxis::doMove(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { // Status of read-write-operations of ASCII commands to the controller asynStatus status = asynSuccess; char value[pC_->MAXBUF_]; double motorCoordinatesPosition = 0.0; double motorRecResolution = 0.0; double motorVelocity = 0.0; int enabled = 0; int motorCanSetSpeed = 0; // ========================================================================= getAxisParamChecked(this, motorEnableRBV, &enabled); getAxisParamChecked(this, motorRecResolution, &motorRecResolution); if (enabled == 0) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nAxis is " "disabled.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); return asynSuccess; } // Convert from EPICS to user / motor units motorCoordinatesPosition = position * motorRecResolution; motorVelocity = maxVelocity * motorRecResolution; asynPrint( pC_->pasynUser(), ASYN_TRACE_FLOW, "Controller \"%s\", axis %d => %s, line %d:\nMove to position %lf.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, position); // Check if the speed is allowed to be changed getAxisParamChecked(this, motorCanSetSpeed, &motorCanSetSpeed); // Initialize the movement handshake status = pC_->write(axisNo_, 86, "0"); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); return status; } // Set the new motor speed, if the user is allowed to do so and if the // motor speed changed since the last move command. if (motorCanSetSpeed != 0) { snprintf(value, sizeof(value), "%lf", motorVelocity); status = pC_->write(axisNo_, 05, value); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); return status; } asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW, "Controller \"%s\", axis %d => %s, line %d:\nSetting speed " "to %lf.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, motorVelocity); } // Set the target position snprintf(value, sizeof(value), "%lf", motorCoordinatesPosition); status = pC_->write(axisNo_, 02, value); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); return status; } // If the motor has just been enabled, use Enable double timeout = pC_->comTimeout(); if (pMasterMacsA_->targetReachedUninitialized && timeout < PowerCycleTimeout) { timeout = PowerCycleTimeout; } // Start the move if (relative) { status = pC_->write(axisNo_, 00, "2", timeout); } else { status = pC_->write(axisNo_, 00, "1", timeout); } if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); return status; } // In the next poll, we will check if the handshake has been performed in a // reasonable time pMasterMacsA_->waitForHandshake = true; pMasterMacsA_->timeAtHandshake = time(NULL); // Waiting for a handshake is already part of the movement procedure => // Start the watchdog if (startMovTimeoutWatchdog() != asynSuccess) { return asynError; } return status; } asynStatus masterMacsAxis::stop(double acceleration) { asynStatus status = pC_->write(axisNo_, 00, "8"); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); } // Reset the driver to idle state and move out of the handshake wait loop, // if we're currently inside it. pMasterMacsA_->waitForHandshake = false; return status; } asynStatus masterMacsAxis::doReset() { asynStatus status = asynSuccess; // Reset the controller ("node reset") status = pC_->write(axisNo_, 16, ""); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); } // Reset any errors in the controller. Since the node reset results in a // power cycle, we use the corresponding timeout. status = pC_->write(axisNo_, 17, "", PowerCycleTimeout); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); } // Move out of the handshake wait loop, if we're currently inside it. pMasterMacsA_->waitForHandshake = false; // Reinitialize the axis status = masterMacsAxis::init(); if (status != asynSuccess) { return status; } bool moving = false; return forcedPoll(&moving); } /* Home the axis. On absolute encoder systems, this is a no-op */ asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity, double acceleration, int forwards) { char response[pC_->MAXBUF_] = {0}; // ========================================================================= getAxisParamChecked(this, encoderType, &response); // Only send the home command if the axis has an incremental encoder if (strcmp(response, IncrementalEncoder) == 0) { // Initialize the movement handshake asynStatus status = pC_->write(axisNo_, 86, "0"); if (status != asynSuccess) { setAxisParamChecked(this, motorStatusProblem, true); return status; } status = pC_->write(axisNo_, 00, "9"); if (status != asynSuccess) { return status; } // In the next poll, we will check if the handshake has been performed // in a reasonable time pMasterMacsA_->waitForHandshake = true; pMasterMacsA_->timeAtHandshake = time(NULL); return asynSuccess; } else { return asynError; } } /* Read the encoder type and update the parameter library accordingly */ asynStatus masterMacsAxis::readEncoderType() { char command[pC_->MAXBUF_] = {0}; char response[pC_->MAXBUF_] = {0}; int nvals = 0; int encoder_id = 0; // ========================================================================= // Check if this is an absolute encoder asynStatus status = pC_->read(axisNo_, 60, response); if (status != asynSuccess) { return status; } nvals = sscanf(response, "%d", &encoder_id); if (nvals != 1) { return pC_->couldNotParseResponse(command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } /* Defined encoder IDs: 0=INC (Incremental) 1=SSI (Absolute encoder with SSI interface) 2=SSI (Absolute encoder with BiSS interface) */ if (encoder_id == 0) { setAxisParamChecked(this, encoderType, IncrementalEncoder); } else { setAxisParamChecked(this, encoderType, AbsoluteEncoder); } return asynSuccess; } asynStatus masterMacsAxis::enable(bool on) { int timeout_enable_disable = 2; char msg[pC_->MAXBUF_]; // ========================================================================= /* When the motor is changing its enable state, its targetReached bit is set to 0. In order to prevent the poll method in interpreting the motor state as "moving", this flag is used. It is reset in the handshake. */ pMasterMacsA_->targetReachedUninitialized = true; /* Continue regardless of the status returned by the poll; we just want to find out whether the motor is currently moving or not. If the poll function fails before it can determine that, it is assumed that the motor is not moving. */ bool moving = false; doPoll(&moving); // If the axis is currently moving, it cannot be disabled. Ignore the // command and inform the user. We check the last known status of the // axis instead of "moving", since status -6 is also moving, but the // motor can actually be disabled in this state! if (moving) { asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nAxis is not " "idle and can therefore not be disabled.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); setAxisParamChecked(this, motorMessageText, "Axis cannot be disabled while it is moving."); return asynError; } // Axis is already enabled / disabled and a new enable / disable command // was sent => Do nothing if ((readyToBeSwitchedOn() && switchedOn()) == on) { asynPrint( pC_->pasynUser(), ASYN_TRACE_WARNING, "Controller \"%s\", axis %d => %s, line %d:\nAxis is already %s.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, on ? "enabled" : "disabled"); return asynSuccess; } // Enable / disable the axis if it is not moving snprintf(msg, sizeof(msg), "%d", on); asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW, "Controller \"%s\", axis %d => %s, line %d:\n%s axis.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, on ? "Enable" : "Disable"); // The answer to the enable command on MasterMACS might take some time, // hence we wait for a custom timespan in seconds instead of // pC_->comTimeout_ double timeout = pC_->comTimeout(); if (pMasterMacsA_->targetReachedUninitialized && timeout < PowerCycleTimeout) { timeout = PowerCycleTimeout; } asynStatus status = pC_->write(axisNo_, 04, msg, timeout); if (status != asynSuccess) { return status; } // Query the axis status every few milliseconds until the axis has been // enabled or until the timeout has been reached int startTime = time(NULL); while (time(NULL) < startTime + timeout_enable_disable) { // Read the axis status usleep(100000); status = readAxisStatus(); if (status != asynSuccess) { return status; } if (switchedOn() == on) { bool moving = false; // Perform a poll to update the parameter library forcedPoll(&moving); return asynSuccess; } } // Failed to change axis status within timeout_enable_disable => Send a // corresponding message asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d:\nFailed to %s axis " "within %d seconds\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, on ? "enable" : "disable", timeout_enable_disable); // Output message to user snprintf(msg, sizeof(msg), "Failed to %s within %d seconds", on ? "enable" : "disable", timeout_enable_disable); setAxisParamChecked(this, motorMessageText, msg); return asynError; } bool masterMacsAxis::needInit() { return pMasterMacsA_->needInit; } /** * @brief Instruct the axis to run its init() function during the next poll * * @param needInit */ void masterMacsAxis::setNeedInit(bool needInit) { pMasterMacsA_->needInit = needInit; } /** Convert a float to an unint16_t bitset */ std::bitset<16> toBitset(float val) { // Convert the 32-bit float to an uint32_t. This works independently of the // endianess of the system. uint32_t temp = (uint32_t)val; // Take the bottom half of bits to convert the 32 bit uint to a 16 bit uint uint16_t lowerHalf = (uint16_t)(temp & 0xFFFF); return std::bitset<16>(lowerHalf); } asynStatus masterMacsAxis::readAxisStatus() { char response[pC_->MAXBUF_] = {0}; // ========================================================================= asynStatus rw_status = pC_->read(axisNo_, 10, response); if (rw_status == asynSuccess) { float axisStatus = 0; int nvals = sscanf(response, "%f", &axisStatus); if (nvals != 1) { return pC_->couldNotParseResponse("R10", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } pMasterMacsA_->axisStatus = toBitset(axisStatus); } return rw_status; } asynStatus masterMacsAxis::readAxisError() { char response[pC_->MAXBUF_] = {0}; // ========================================================================= asynStatus rw_status = pC_->read(axisNo_, 11, response); if (rw_status == asynSuccess) { float axisError = 0; int nvals = sscanf(response, "%f", &axisError); if (nvals != 1) { return pC_->couldNotParseResponse("R11", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } pMasterMacsA_->axisError = toBitset(axisError); } return rw_status; } bool masterMacsAxis::readyToBeSwitchedOn() { return pMasterMacsA_->axisStatus[0]; } bool masterMacsAxis::switchedOn() { return pMasterMacsA_->axisStatus[1]; } bool masterMacsAxis::faultConditionSet() { return pMasterMacsA_->axisStatus[3]; } bool masterMacsAxis::voltagePresent() { return pMasterMacsA_->axisStatus[4]; } bool masterMacsAxis::quickStopping() { return pMasterMacsA_->axisStatus[5] == 0; } bool masterMacsAxis::switchOnDisabled() { return pMasterMacsA_->axisStatus[6]; } bool masterMacsAxis::warning() { return pMasterMacsA_->axisStatus[7]; } bool masterMacsAxis::remoteMode() { return pMasterMacsA_->axisStatus[9]; } bool masterMacsAxis::targetReached() { return pMasterMacsA_->axisStatus[10]; } bool masterMacsAxis::internalLimitActive() { return pMasterMacsA_->axisStatus[11]; } bool masterMacsAxis::setEventHasOcurred() { return pMasterMacsA_->axisStatus[14]; } bool masterMacsAxis::powerEnabled() { return pMasterMacsA_->axisStatus[15]; } bool masterMacsAxis::shortCircuit() { return pMasterMacsA_->axisError[1]; } bool masterMacsAxis::encoderError() { return pMasterMacsA_->axisError[2]; } bool masterMacsAxis::followingError() { return pMasterMacsA_->axisError[3]; } bool masterMacsAxis::communicationError() { return pMasterMacsA_->axisError[4]; } bool masterMacsAxis::feedbackError() { return pMasterMacsA_->axisError[5]; } bool masterMacsAxis::positiveLimitSwitch() { return pMasterMacsA_->axisError[6]; } bool masterMacsAxis::negativeLimitSwitch() { return pMasterMacsA_->axisError[7]; } bool masterMacsAxis::positiveSoftwareLimit() { return pMasterMacsA_->axisError[8]; } bool masterMacsAxis::negativeSoftwareLimit() { return pMasterMacsA_->axisError[9]; } bool masterMacsAxis::overCurrent() { return pMasterMacsA_->axisError[10]; } bool masterMacsAxis::overTemperature() { return pMasterMacsA_->axisError[11]; } bool masterMacsAxis::overVoltage() { return pMasterMacsA_->axisError[12]; } bool masterMacsAxis::underVoltage() { return pMasterMacsA_->axisError[13]; } bool masterMacsAxis::stoFault() { return pMasterMacsA_->axisError[15]; } /***************************************************************************/ /** The following functions are C-wrappers, and can be called directly from * iocsh */ extern "C" { /* C wrapper for the axis constructor. Please refer to the masterMacsAxis constructor documentation. The controller is read from the portName. */ asynStatus masterMacsCreateAxis(const char *portName, int axis) { /* findAsynPortDriver is a asyn library FFI function which uses the C ABI. Therefore it returns a void pointer instead of e.g. a pointer to a superclass of the controller such as asynPortDriver. Type-safe upcasting via dynamic_cast is therefore not possible directly. However, we do know that the void pointer is either a pointer to asynPortDriver (if a driver with the specified name exists) or a nullptr. Therefore, we first do a nullptr check, then a cast to asynPortDriver and lastly a (typesafe) dynamic_upcast to Controller */ void *ptr = findAsynPortDriver(portName); if (ptr == nullptr) { /* We can't use asynPrint here since this macro would require us to get a lowLevelPortUser_ from a pointer to an asynPortDriver. However, the given pointer is a nullptr and therefore doesn't have a lowLevelPortUser_! printf is an EPICS alternative which works w/o that, but doesn't offer the comfort provided by the asynTrace-facility */ errlogPrintf("Controller \"%s\" => %s, line %d:\nPort %s not found.", portName, __PRETTY_FUNCTION__, __LINE__, portName); return asynError; } // Unsafe cast of the pointer to an asynPortDriver asynPortDriver *apd = (asynPortDriver *)(ptr); // Safe downcast masterMacsController *pC = dynamic_cast(apd); if (pC == nullptr) { errlogPrintf("Controller \"%s\" => %s, line %d:\nController is not a " "masterMacsController.", portName, __PRETTY_FUNCTION__, __LINE__); return asynError; } // Prevent manipulation of the controller from other threads while we // create the new axis. pC->lock(); /* We create a new instance of the axis, using the "new" keyword to allocate it on the heap while avoiding RAII. The created object is registered in EPICS in its constructor and can safely be "leaked" here. */ #pragma GCC diagnostic ignored "-Wunused-but-set-variable" #pragma GCC diagnostic ignored "-Wunused-variable" masterMacsAxis *pAxis = new masterMacsAxis(pC, axis); // Allow manipulation of the controller again pC->unlock(); return asynSuccess; } /* This is boilerplate code which is used to make the FFI functions CreateController and CreateAxis "known" to the IOC shell (iocsh). */ #ifdef vxWorks #else /* Same procedure as for the CreateController function, but for the axis itself. */ static const iocshArg CreateAxisArg0 = {"Controller name (e.g. mmacs1)", iocshArgString}; static const iocshArg CreateAxisArg1 = {"Axis number", iocshArgInt}; static const iocshArg *const CreateAxisArgs[] = {&CreateAxisArg0, &CreateAxisArg1}; static const iocshFuncDef configMasterMacsCreateAxis = {"masterMacsAxis", 2, CreateAxisArgs}; static void configMasterMacsCreateAxisCallFunc(const iocshArgBuf *args) { masterMacsCreateAxis(args[0].sval, args[1].ival); } // This function is made known to EPICS in masterMacs.dbd and is called by // EPICS in order to register both functions in the IOC shell static void masterMacsAxisRegister(void) { iocshRegister(&configMasterMacsCreateAxis, configMasterMacsCreateAxisCallFunc); } epicsExportRegistrar(masterMacsAxisRegister); #endif } // extern "C"