Merge pull request #4 from epics-motor/issue2

Allow incremental encoders to be used with piezo stages
epics-motor · May 12, 2020 · 5f3583e · 5f3583e
2 parents 498e72b + 16557f0
commit 5f3583e
Show file tree

Hide file tree

Showing 5 changed files with 127 additions and 26 deletions.
diff --git 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
@@ -1,14 +1,14 @@
 /*
-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
 * found in the file LICENSE that is included with this distribution.
 *************************************************************************
 
 
-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
@@ -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
@@ -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
@@ -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
 
 };