diff --git a/src/masterMacsAxis.cpp b/src/masterMacsAxis.cpp index 254040e..2137772 100644 --- a/src/masterMacsAxis.cpp +++ b/src/masterMacsAxis.cpp @@ -11,6 +11,11 @@ #include #include +enum moveMode { + positionMode, + velocityMode, +}; + struct masterMacsAxisImpl { /* The axis status and axis error of MasterMACS are given as an integer from @@ -24,6 +29,9 @@ struct masterMacsAxisImpl { bool needInit = true; bool targetReachedUninitialized; bool dynamicLimits; + bool canPositionMove; + bool canVelocityMove; + moveMode lastMoveCommand; }; /* @@ -94,6 +102,9 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo) .timeAtHandshake = 0, .targetReachedUninitialized = true, .dynamicLimits = false, + .canPositionMove = true, + .canVelocityMove = false, + .lastMoveCommand = positionMode, })) { asynStatus status = asynSuccess; @@ -190,9 +201,8 @@ asynStatus masterMacsAxis::init() { double motVelocity = 0.0; double motVmax = 0.0; double motAccel = 0.0; - int motMode = 0; int dynamicLimits = 0; - int motCanSetMode = 0; + int possibleModes = 0; // ========================================================================= @@ -306,36 +316,38 @@ asynStatus masterMacsAxis::init() { } pMasterMacsA_->dynamicLimits = bool(dynamicLimits); - // Check the current motor mode - status = pC_->read(axisNo_, 07, response); - if (status != asynSuccess) { - return status; - } - nvals = sscanf(response, "%d", &motMode); - if (nvals != 1) { - return pC_->couldNotParseResponse("R07", response, axisNo_, - __PRETTY_FUNCTION__, __LINE__); - } - - // If the readback value from the controller is 3, it is in velocity mode, - // which sinqMotor encodes as a 1. Otherwise, it is in position mode. - setAxisParamChecked(this, motorMode, motMode == 3); - // Check if the motor can switch its mode status = pC_->read(axisNo_, 31, response); if (status != asynSuccess) { return status; } - nvals = sscanf(response, "%d", &motCanSetMode); + nvals = sscanf(response, "%d", &possibleModes); if (nvals != 1) { return pC_->couldNotParseResponse("R31", response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } - // If the readback value from the controller is 3, the motor supports both - // velocity and position mode, otherwise just one of them (the one read out - // with motMode). - setAxisParamChecked(this, motorCanSetMode, motCanSetMode == 3); + switch (possibleModes) { + case 1: + pMasterMacsA_->canPositionMove = true; + pMasterMacsA_->canVelocityMove = false; + break; + case 2: + pMasterMacsA_->canPositionMove = false; + pMasterMacsA_->canVelocityMove = true; + break; + case 3: + pMasterMacsA_->canPositionMove = true; + pMasterMacsA_->canVelocityMove = true; + break; + default: + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d:\nunexpected answer %d for R31 (possible operation " + "modes). Expected one of 1, 2 or 3.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + possibleModes); + } status = readEncoderType(); if (status != asynSuccess) { @@ -432,10 +444,10 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { asynStatus poll_status = asynSuccess; // Status of read-write-operations of ASCII commands to the controller - asynStatus rw_status = asynSuccess; + asynStatus rwStatus = asynSuccess; // Status of parameter library operations - asynStatus pl_status = asynSuccess; + asynStatus plStatus = asynSuccess; char response[pC_->MAXBUF_] = {0}; int nvals = 0; @@ -446,6 +458,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { double previousPosition = 0.0; double motorRecResolution = 0.0; double handshakePerformed = 0; + double actualVelocity = 0.0; // ========================================================================= @@ -484,8 +497,8 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { } pC_->read(axisNo_, 86, response); - if (rw_status != asynSuccess) { - return rw_status; + if (rwStatus != asynSuccess) { + return rwStatus; } nvals = sscanf(response, "%lf", &handshakePerformed); @@ -511,15 +524,15 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { getAxisParamChecked(this, motorRecResolution, &motorRecResolution); // Read the previous motor position - pl_status = motorPosition(&previousPosition); - if (pl_status != asynSuccess) { - return pl_status; + plStatus = motorPosition(&previousPosition); + if (plStatus != asynSuccess) { + return plStatus; } // Update the axis status - rw_status = readAxisStatus(); - if (rw_status != asynSuccess) { - return rw_status; + rwStatus = readAxisStatus(); + if (rwStatus != asynSuccess) { + return rwStatus; } // If we wait for a handshake, but the motor was moving in its last poll @@ -539,23 +552,37 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { 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__); + // If moving in velocity mode, read out the current speed. Otherwise, read + // out the current position + if (pMasterMacsA_->lastMoveCommand == velocityMode && *moving) { + rwStatus = pC_->read(axisNo_, 14, response); + if (rwStatus != asynSuccess) { + return rwStatus; + } + nvals = sscanf(response, "%lf", &actualVelocity); + if (nvals != 1) { + return pC_->couldNotParseResponse("R14", response, axisNo_, + __PRETTY_FUNCTION__, __LINE__); + } + + } else { + rwStatus = pC_->read(axisNo_, 12, response); + if (rwStatus != asynSuccess) { + return rwStatus; + } + 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. + Read out the error if either a fault condition status flag has been set + or if a movement has just ended. */ if (faultConditionSet() || !(*moving)) { - rw_status = readAxisError(); + rwStatus = readAxisError(); } msgPrintControlKey keyError = msgPrintControlKey( @@ -735,9 +762,9 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { // Read out the limits, if the motor is not moving and if the limits are // dynamic if (pMasterMacsA_->dynamicLimits && !(*moving)) { - rw_status = readLimits(); - if (rw_status != asynSuccess) { - return rw_status; + rwStatus = readLimits(); + if (rwStatus != asynSuccess) { + return rwStatus; } } @@ -762,16 +789,19 @@ asynStatus masterMacsAxis::doPoll(bool *moving) { setAxisParamChecked(this, motorStatusDone, !(*moving)); setAxisParamChecked(this, motorStatusDirection, direction); - pl_status = setMotorPosition(currentPosition); - if (pl_status != asynSuccess) { - return pl_status; + plStatus = setMotorPosition(currentPosition); + if (plStatus != asynSuccess) { + return plStatus; } + + // Write the actual velocity to the paramLib + setAxisParamChecked(this, motorVelocity, actualVelocity); + return poll_status; } -asynStatus masterMacsAxis::doMoveVelocity(double minVelocity, - double maxVelocity, - double acceleration) { +asynStatus masterMacsAxis::moveVelocity(double minVelocity, double maxVelocity, + double acceleration) { // Suppress unused variable warning (void)minVelocity; (void)acceleration; @@ -787,6 +817,18 @@ asynStatus masterMacsAxis::doMoveVelocity(double minVelocity, // ========================================================================= + // Can the motor be operated in velocity mode? + if (!pMasterMacsA_->canVelocityMove) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nAxis cannot " + "operate in velocity mode.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + setAxisParamChecked(this, motorStatusProblem, true); + setAxisParamChecked(this, motorMessageText, + "cannot operate in velocity mode"); + return asynError; + } + getAxisParamChecked(this, motorEnableRBV, &enabled); getAxisParamChecked(this, motorRecResolution, &motorRecResolution); @@ -822,7 +864,14 @@ asynStatus masterMacsAxis::doMoveVelocity(double minVelocity, // Start the move. We do not use the MovTimeout watchdog here, because the // motor can move for any time in velocity mode. - return pC_->write(axisNo_, 00, "3", timeout); + status = pC_->write(axisNo_, 00, "3", timeout); + if (status != asynSuccess) { + return status; + } + + // Cache the information that the current movement is in velocity mode + pMasterMacsA_->lastMoveCommand = velocityMode; + return status; } asynStatus masterMacsAxis::doMove(double position, int relative, @@ -845,6 +894,18 @@ asynStatus masterMacsAxis::doMove(double position, int relative, // ========================================================================= + // Can the motor be operated in position mode? + if (!pMasterMacsA_->canPositionMove) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d:\nAxis cannot " + "operate in position mode.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + setAxisParamChecked(this, motorStatusProblem, true); + setAxisParamChecked(this, motorMessageText, + "cannot operate in position mode"); + return asynError; + } + getAxisParamChecked(this, motorEnableRBV, &enabled); getAxisParamChecked(this, motorRecResolution, &motorRecResolution); @@ -930,6 +991,8 @@ asynStatus masterMacsAxis::doMove(double position, int relative, return asynError; } + // Cache the information that the current movement is in position mode + pMasterMacsA_->lastMoveCommand = positionMode; return status; } @@ -1230,8 +1293,8 @@ asynStatus masterMacsAxis::readAxisStatus() { // ========================================================================= - asynStatus rw_status = pC_->read(axisNo_, 10, response); - if (rw_status == asynSuccess) { + asynStatus rwStatus = pC_->read(axisNo_, 10, response); + if (rwStatus == asynSuccess) { float axisStatus = 0; int nvals = sscanf(response, "%f", &axisStatus); @@ -1243,7 +1306,7 @@ asynStatus masterMacsAxis::readAxisStatus() { pMasterMacsA_->axisStatus = toBitset(axisStatus); } - return rw_status; + return rwStatus; } asynStatus masterMacsAxis::readAxisError() { @@ -1251,8 +1314,8 @@ asynStatus masterMacsAxis::readAxisError() { // ========================================================================= - asynStatus rw_status = pC_->read(axisNo_, 11, response); - if (rw_status == asynSuccess) { + asynStatus rwStatus = pC_->read(axisNo_, 11, response); + if (rwStatus == asynSuccess) { float axisError = 0; int nvals = sscanf(response, "%f", &axisError); @@ -1262,7 +1325,7 @@ asynStatus masterMacsAxis::readAxisError() { } pMasterMacsA_->axisError = toBitset(axisError); } - return rw_status; + return rwStatus; } bool masterMacsAxis::readyToBeSwitchedOn() { diff --git a/src/masterMacsAxis.h b/src/masterMacsAxis.h index 13f1dc5..559a633 100644 --- a/src/masterMacsAxis.h +++ b/src/masterMacsAxis.h @@ -52,17 +52,15 @@ class HIDDEN masterMacsAxis : public sinqAxis { asynStatus doPoll(bool *moving); /** - * @brief Implementation of the `doMoveVelocity` function from sinqAxis. The - * parameters are described in the documentation of - * `sinqAxis::doMoveVelocity`. + * @brief TODO * * @param minVelocity * @param maxVelocity * @param acceleration * @return asynStatus */ - asynStatus doMoveVelocity(double minVelocity, double maxVelocity, - double acceleration); + asynStatus moveVelocity(double minVelocity, double maxVelocity, + double acceleration); /** * @brief Implementation of the `doMove` function from sinqAxis. The