diff --git a/pigcs2App/src/PIGCSController.cpp b/pigcs2App/src/PIGCSController.cpp index d97b7d1..aceab9d 100644 --- a/pigcs2App/src/PIGCSController.cpp +++ b/pigcs2App/src/PIGCSController.cpp @@ -169,7 +169,7 @@ asynStatus PIGCSController::setAxisPositionCts(PIasynAxis* pAxis, double positio double position = double(positionCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator; asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR, - "PIGCSController::setAxisPositionCts(, %d) \n", positionCts); + "PIGCSController::setAxisPositionCts(, %f) \n", positionCts); return setAxisPosition(pAxis, position); } @@ -711,7 +711,7 @@ asynStatus PIGCSController::getReferencedState(PIasynAxis* pAxis) { return status; } - if (getValue(buf, pAxis->m_homed)) + if (!getValue(buf, pAxis->m_homed)) { return asynError; } @@ -786,6 +786,15 @@ bool PIGCSController::getValue(const char* szMsg, bool& value) return false; } int ivalue = atoi(p+1); - value = (ivalue =! 0); + value = (ivalue != 0); return true; } + +void PIGCSController::getStatusFromBitMask (long mask, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl) +{ + moving = (mask & 0x2000) ? 1 : 0; + homing = (mask & 0x4000) ? 1 : 0; + negLimit = (mask & 0x0001) ? 1 : 0; + posLimit = (mask & 0x0004) ? 1 : 0; + servoControl = (mask & 0x1000) ? 1 : 0; +} diff --git a/pigcs2App/src/PIGCSController.h b/pigcs2App/src/PIGCSController.h index d16e7eb..80e217a 100644 --- a/pigcs2App/src/PIGCSController.h +++ b/pigcs2App/src/PIGCSController.h @@ -1,6 +1,6 @@ /* -FILENAME... PIGCScontroller.h - +FILENAME... PIGCScontroller.h + ************************************************************************* * Copyright (c) 2011-2013 Physik Instrumente (PI) GmbH & Co. KG * This file is distributed subject to the EPICS Open License Agreement @@ -8,7 +8,7 @@ FILENAME... PIGCScontroller.h ************************************************************************* -Original Author: Steffen Rau +Original Author: Steffen Rau Created: 15.12.2010 */ @@ -51,7 +51,7 @@ class PIGCSController virtual asynStatus move( PIasynAxis* pAxis, double target); virtual asynStatus moveCts( PIasynAxis* pAxis, int target); virtual asynStatus moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes); - virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards) { return asynSuccess; } + virtual asynStatus referenceVelCts (PIasynAxis* pAxis, double velocity, int forwards) = 0; virtual asynStatus haltAxis(PIasynAxis* pAxis); @@ -97,6 +97,9 @@ class PIGCSController PIInterface* m_pInterface; static const size_t MAX_NR_AXES = 64; bool m_bAnyAxisMoving; + + static void getStatusFromBitMask(long mask, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl); + protected: asynStatus setGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double value); asynStatus getGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double& value); diff --git a/pigcs2App/src/PIGCSMotorController.cpp b/pigcs2App/src/PIGCSMotorController.cpp index 78e320f..52cde6e 100644 --- a/pigcs2App/src/PIGCSMotorController.cpp +++ b/pigcs2App/src/PIGCSMotorController.cpp @@ -1,5 +1,5 @@ /* -FILENAME... PIGCSMotorController.cpp +FILENAME... PIGCSMotorController.cpp ************************************************************************* * Copyright (c) 2011-2013 Physik Instrumente (PI) GmbH & Co. KG @@ -8,7 +8,7 @@ FILENAME... PIGCSMotorController.cpp ************************************************************************* -Original Author: Steffen Rau +Original Author: Steffen Rau Created: 15.12.2010 */ @@ -141,19 +141,14 @@ asynStatus PIGCSMotorController::getStatus(PIasynAxis* pAxis, int& homing, int& { return status; } - // TODO this is for a single axis C-863/867 controller!!!! - // TODO a) change it to multi-axis code. - // TODO b) support other controllers which do not understand #4 or have different bit masks + // TODO this is for C-863/867 controllers!!!! + // TODO support other controllers which do not understand #4 or have different bit masks int idx = 2 + pAxis->getAxisNo()*4; buf[idx+4] = '\0'; char* szMask = buf+idx; long mask = strtol(szMask, NULL, 16); - moving = (mask & 0x2000) ? 1 : 0; - homing = (mask & 0x4000) ? 1 : 0; - negLimit = (mask & 0x0001) ? 1 : 0; - posLimit = (mask & 0x0004) ? 1 : 0; - servoControl = (mask & 0x1000) ? 1 : 0; + getStatusFromBitMask (mask, homing, moving, negLimit, posLimit, servoControl); asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW, "PIGCSMotorController::getStatus() buf:%s moving %d, svo: %d\n", buf, moving, servoControl); diff --git a/pigcs2App/src/PIGCSPiezoController.cpp b/pigcs2App/src/PIGCSPiezoController.cpp index 229e0d9..25d2372 100644 --- a/pigcs2App/src/PIGCSPiezoController.cpp +++ b/pigcs2App/src/PIGCSPiezoController.cpp @@ -8,7 +8,7 @@ FILENAME... PIGCSPiezoController.cpp ************************************************************************* -Original Author: Steffen Rau +Original Author: Steffen Rau Created: 15.12.2010 */ @@ -24,30 +24,78 @@ Created: 15.12.2010 asynStatus PIGCSPiezoController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl) { - asynStatus status = getMoving(pAxis, moving); + char cmd[100]; + char buf[255]; + sprintf(cmd, "SRG? %s 1", pAxis->m_szAxisName); + asynStatus status = m_pInterface->sendAndReceive(cmd, buf, 99); if (status != asynSuccess) { - return status; + return status; } - homing = 0; - negLimit = 0; - posLimit = 0; + const char* p = strstr(buf, "="); + if (p==NULL || *p == '\0') + { + return asynError; + } + + long mask = strtol(p+1, NULL, 0); + getStatusFromBitMask (mask, homing, moving, negLimit, posLimit, servoControl); + // TODO: use parameter or "LIM?" to find out if stage has limit switches + negLimit = 0; + posLimit = 0; + asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW, + "PIGCSPiezoController::getStatus() buf:%s moving %d, svo: %d\n", + buf, moving, servoControl); return status; } asynStatus PIGCSPiezoController::getReferencedState(PIasynAxis* pAxis) { + if (m_hasqFRF) + { + asynStatus status = PIGCSController::getReferencedState (pAxis); + if (asynSuccess == status) + { + return status; + } + if (getGCSError () != PI_CNTR_UNKNOWN_COMMAND__2) + { + return asynError; + } + m_hasqFRF = false; + } pAxis->m_homed = 1; return asynSuccess; } asynStatus PIGCSPiezoController::initAxis(PIasynAxis* pAxis) { + pAxis->m_bHasReference = false; + + if (m_hasqTRS) + { + asynStatus status = hasReferenceSensor (pAxis); + if (asynSuccess != status) + { + if (getGCSError () != PI_CNTR_UNKNOWN_COMMAND__2) + { + return asynError; + } + m_hasqTRS = false; + } + } + pAxis->m_movingStateMask = pow(2.0, pAxis->getAxisNo()); - return setServo(pAxis, 1); + // enable servo only if axis is homed + int servoState = 1; + if (asynSuccess == getReferencedState (pAxis)) + { + servoState = pAxis->m_homed; + } + return setServo(pAxis, servoState); } /** @@ -80,4 +128,45 @@ asynStatus PIGCSPiezoController::haltAxis(PIasynAxis* pAxis) return status; } +asynStatus PIGCSPiezoController::referenceVelCts (PIasynAxis* pAxis, double velocity, int forwards) +{ + if (!m_hasqFRF) + { + // device does not know how to reference, many piezo stages have absolute sensors + return asynSuccess; + } + + asynStatus status = setServo(pAxis, 0); // piezo controllers need the servo to be disabled + if (asynSuccess != status) + return status; + + char cmd[100]; + if (pAxis->m_bHasReference) + { + // call FRF - find reference + sprintf(cmd,"FRF %s", pAxis->m_szAxisName); + } + else + { + asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR, + "PIGCSPiezoController::referenceVelCts() failed - axis has no reference switch\n"); + epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize, + "PIGCSPiezoController::referenceVelCts() failed - axis has no reference switch\n"); + return asynError; + } + status = m_pInterface->sendOnly(cmd); + if (asynSuccess != status) + return status; + int errorCode = getGCSError(); + if (errorCode == 0) + { + return asynSuccess; + } + asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR, + "PIGCSPiezoController::referenceVelCts() failed\n"); + epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize, + "PIGCSPiezoController::referenceVelCts() failed - GCS Error %d\n",errorCode); + return asynError; + +} diff --git a/pigcs2App/src/PIGCSPiezoController.h b/pigcs2App/src/PIGCSPiezoController.h index 0b13581..51dc7c7 100644 --- a/pigcs2App/src/PIGCSPiezoController.h +++ b/pigcs2App/src/PIGCSPiezoController.h @@ -8,7 +8,7 @@ FILENAME... PIGCScontroller.h ************************************************************************* -Original Author: Steffen Rau +Original Author: Steffen Rau Created: 15.12.2010 */ @@ -27,7 +27,9 @@ class PIGCSPiezoController : public PIGCSController { public: PIGCSPiezoController(PIInterface* pInterface, const char* szIDN) - : PIGCSController(pInterface, szIDN) + : PIGCSController (pInterface, szIDN) + , m_hasqFRF (true) + , m_hasqTRS (true) { } ~PIGCSPiezoController() {} @@ -39,9 +41,12 @@ class PIGCSPiezoController : public PIGCSController virtual asynStatus setAxisPosition(PIasynAxis* pAxis, double position); virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl); virtual asynStatus getReferencedState(PIasynAxis* pAxis); + virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards); private: + bool m_hasqFRF; ///< is "FRF?" command available + bool m_hasqTRS; ///< is "TRS?" command available };