Added changes to pigcs2App from epics-motor#1

kmpeters · Apr 29, 2019 · 035a937 · 035a937
1 parent 59b31e0
commit 035a937
Show file tree

Hide file tree

Showing 7 changed files with 144 additions and 19 deletions.
diff --git a/pigcs2App/Db/Makefile b/pigcs2App/Db/Makefile
@@ -14,6 +14,7 @@ include $(TOP)/configure/CONFIG
 
 DB += PI_SupportCtrl.db
 DB += PI_Support.db
+DB += PI_PIGCSasynController.db
 
 #----------------------------------------------------
 # If <anyname>.db template is not named <anyname>*.template add

diff --git a/pigcs2App/Db/PI_PIGCSasynController.db b/pigcs2App/Db/PI_PIGCSasynController.db
@@ -0,0 +1,51 @@
+record(ao, "$(P):$(M)")
+{
+    field(DESC, "$(DESC)")
+    field(DTYP, "asynFloat64")
+    field(PINI, "NO")
+    field(OUT, "@asyn($(PORT),$(ADDR),$(TIMEOUT))MOTOR_MOVE_ABS")
+}
+
+record(ai, "$(P):$(M):RBV")
+{
+    field(DESC, "Read Back Value of Axis")
+    field(DTYP, "asynFloat64")
+    field(INP, "@asyn($(PORT),$(ADDR),$(TIMEOUT))PI_SUP_POSITION")
+    field(PREC, "9")
+    field(SCAN, "I/O Intr")
+}
+
+record(ai, "$(P):$(M):RBV_EGU")
+{
+    field(DESC, "Read Back Value in ums")
+    field(DTYP, "asynFloat64")
+    field(INP, "@asyn($(PORT),$(ADDR),$(TIMEOUT))PI_SUP_POSITION_EGU")
+    field(PREC, "9")
+    field(SCAN, "I/O Intr")
+}
+
+record(ao, "$(P):$(M):VELO")
+{
+    field(DESC, "Velocity")
+    field(DTYP, "asynFloat64")
+    field(VAL, "$(VELO)")
+    field(PINI, "NO")
+    field(OUT, "@asyn($(PORT),$(ADDR),$(TIMEOUT))MOTOR_VELOCITY")
+}
+
+record(ao, "$(P):$(M):ACCL")
+{
+    field(DESC, "Acceleration")
+    field(DTYP, "asynFloat64")
+    field(VAL, "$(ACCL)")
+    field(PINI, "NO")
+    field(OUT, "@asyn($(PORT),$(ADDR),$(TIMEOUT))MOTOR_ACCEL")
+}
+
+record(bo, "$(P):$(M):STOP")
+{
+    field(DESC, "Stop")
+    field(DTYP, "asynInt32")
+    field(PINI, "NO")
+    field(OUT, "@asyn($(PORT),$(ADDR),$(TIMEOUT))MOTOR_STOP_AXIS")
+}
diff --git a/pigcs2App/src/PIGCSController.cpp b/pigcs2App/src/PIGCSController.cpp
@@ -113,19 +113,44 @@ bool PIGCSController::IsGCS2(PIInterface* pInterface)
 
 asynStatus PIGCSController::setVelocityCts( PIasynAxis* pAxis, double velocity )
 {
-    if (!m_KnowsVELcommand)
-    {
-        return asynSuccess;
-    }
 	char cmd[100];
 	velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
     sprintf(cmd,"VEL %s %f", pAxis->m_szAxisName, velocity);
+    pAxis->m_velocity = velocity;
     asynStatus status = m_pInterface->sendOnly(cmd);
-    if (asynSuccess == status)
+    if (asynSuccess != status)
     {
-    	pAxis->m_velocity = velocity;
+    	return status; 
     }
-    return status;
+    int errorCode = getGCSError();
+    if (errorCode == 0)
+    	return asynSuccess;
+
+    asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
+    		"PIGCSController::setVelocityCts failed, GCS error %d\n", errorCode);
+
+    return asynError;
+}
+
+asynStatus PIGCSController::setAccelerationCts( PIasynAxis* pAxis, double acceleration)
+{
+    char cmd[100];
+    acceleration = fabs(acceleration) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
+    sprintf(cmd,"ACC %s %f", pAxis->m_szAxisName, acceleration);
+    asynStatus status = m_pInterface->sendOnly(cmd);
+    pAxis->m_acceleration = acceleration;
+    if (asynSuccess != status)
+    {
+        return status; 
+    }
+    int errorCode = getGCSError();
+    if (errorCode == 0)
+    	return asynSuccess;
+
+    asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
+    		"PIGCSController::setAccelerationCts failed, GCS error %d\n", errorCode);
+
+    return asynError; 
 }
 
 asynStatus PIGCSController::moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes)
@@ -163,7 +188,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);
 }
 
@@ -206,13 +231,20 @@ asynStatus PIGCSController::setAxisPosition(PIasynAxis* pAxis, double position)
 
 }
 
-
-asynStatus PIGCSController::moveCts( PIasynAxis* pAxis, int targetCts )
+asynStatus PIGCSController::moveCts( PIasynAxis* pAxis, double lowLimit, double highLimit, double targetCts )
 {
-	double target = double(targetCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
-    asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
-    		"PIGCSController::moveCts(, %d) \n", targetCts);
+    double target;
+    if(targetCts !=0) target = targetCts * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
+    else              target = targetCts;
+    if(target >= lowLimit && target <= highLimit)
+    {
+        asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
+        "PIGCSController::moveCts(, %f) \n", targetCts);
 	return move(pAxis, target);
+    }
+
+    asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR, "PIGCSController::moveCts() failed, out of limits target position\n");
+    return asynError;
 }
 
 asynStatus PIGCSController::move( PIasynAxis* pAxis, double target )
@@ -312,6 +344,27 @@ asynStatus PIGCSController::getAxisPosition(PIasynAxis* pAxis, double& position)
 	return status;
 }
 
+asynStatus PIGCSController::getAxisPositionEGU(int inputSignalChannel, double& position)
+{
+    const char* szIdentification = (char*) this->szIdentification;
+    if(strstr(szIdentification, "E-727") != NULL){
+	char cmd[100];
+	char buf[255];
+	sprintf(cmd, "TSP? %d", (inputSignalChannel+1));
+	asynStatus status = m_pInterface->sendAndReceive(cmd, buf, 99);
+	if (status != asynSuccess)
+	{
+		return status;
+	}
+	if (!getValue(buf, position))
+	{
+		status = asynError;
+	}
+	return status;
+    }
+    return asynSuccess;
+}
+
 /**
  *  get velocity of axis in physical units (EGU) as defined on the controller
  *  and set PIasynAxis::m_velocity

diff --git a/pigcs2App/src/PIGCSController.h b/pigcs2App/src/PIGCSController.h
@@ -46,10 +46,10 @@ class PIGCSController
     bool getValue(const char* szMsg, bool& value);
 
     virtual asynStatus setVelocityCts( PIasynAxis* pAxis, double velocity );
-	virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration)	{ return asynSuccess; }
+	virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration);
 	virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration)	{ return asynSuccess; }
 	virtual asynStatus move( PIasynAxis* pAxis, double target);
-	virtual asynStatus moveCts( PIasynAxis* pAxis, int target);
+	virtual asynStatus moveCts( PIasynAxis* pAxis, double lowLimit, double highLimit, double target);
 	virtual asynStatus moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes);
 	virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards) { return asynSuccess;	}
 	virtual asynStatus haltAxis(PIasynAxis* pAxis);
@@ -59,6 +59,7 @@ class PIGCSController
     virtual asynStatus setAxisPosition(PIasynAxis* pAxis, double position);
 
     virtual asynStatus getAxisPosition(PIasynAxis* pAxis, double& position);
+    virtual asynStatus getAxisPositionEGU(int inputSignalChannel, double& position);
     virtual asynStatus getAxisVelocity(PIasynAxis* pAxis);
     virtual asynStatus getAxisPositionCts(PIasynAxis* pAxis);
     virtual asynStatus setServo(PIasynAxis* pAxis, int servoState);

diff --git a/pigcs2App/src/PIasynAxis.cpp b/pigcs2App/src/PIasynAxis.cpp
@@ -90,8 +90,14 @@ void PIasynAxis::Init(const char *portName)
 	m_pGCSController->getAxisVelocity(this);
 	m_pGCSController->getAxisPositionCts(this);
 	setDoubleParam(pController_->motorPosition_, m_positionCts);
+        m_pGCSController->setAxisPosition(this, m_positionCts);
 	setDoubleParam(pController_->motorMoveAbs_, m_positionCts);
-	m_pGCSController->getTravelLimits(this, negLimit_, posLimit_);
+	status = m_pGCSController->getTravelLimits(this, negLimit_, posLimit_);
+        if(asynError == status){
+            asynPrint(logSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR, 
+                        "PIasynController::configAxis() axis %d - getTravelLimits() failed aborting initialization\n", this->m_szAxisName); //abort in case we don't know the travel limits
+            return;
+        }
 	setDoubleParam(pController_->motorLowLimit_, negLimit_);
 	setDoubleParam(pController_->motorHighLimit_, posLimit_);
 	m_pGCSController->getReferencedState(this);
@@ -140,7 +146,9 @@ asynStatus PIasynAxis::poll(bool *returnMoving)
 			m_pGCSController->getAxisPositionCts(this);
 			double realPosition;
 			m_pGCSController->getAxisPosition(this, realPosition);
-			setDoubleParam(pController_->PI_SUP_POSITION,      realPosition );
+                        setDoubleParam(pController_->PI_SUP_POSITION, realPosition);
+                        m_pGCSController->getAxisPositionEGU(this->axisNo_, realPosition);
+                        setDoubleParam(pController_->PI_SUP_POSITION_EGU, realPosition);
 		}
     }
     if (m_isHoming)
@@ -220,7 +228,7 @@ asynStatus PIasynAxis::move(double position, int relative, double minVelocity, d
 		setIntegerParam(pController_->motorStatusDone_, 0);
 		callParamCallbacks();
 
-		status = m_pGCSController->moveCts(this, position);
+		status = m_pGCSController->moveCts(this, this->negLimit_, this->posLimit_, position);
    }
     epicsEventSignal(pController_->pollEventId_);
 

diff --git a/pigcs2App/src/PIasynController.cpp b/pigcs2App/src/PIasynController.cpp
@@ -62,6 +62,7 @@ PIasynController::PIasynController(const char *portName, const char* asynPort, i
 	, m_pGCSController( NULL )
 {
     createParam(PI_SUP_POSITION_String,		asynParamFloat64,	&PI_SUP_POSITION);
+    createParam(PI_SUP_POSITION_EGU_String,	asynParamFloat64,	&PI_SUP_POSITION_EGU);
     createParam(PI_SUP_TARGET_String,		asynParamFloat64,	&PI_SUP_TARGET);
     createParam(PI_SUP_SERVO_String,		asynParamInt32,		&PI_SUP_SERVO);
     createParam(PI_SUP_LAST_ERR_String,		asynParamInt32,		&PI_SUP_LAST_ERR);
@@ -317,7 +318,15 @@ asynStatus PIasynController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu
      * status at the end, but that's OK */
     status = pAxis->setDoubleParam(function, value);
 
-    if (function == PI_SUP_TARGET)
+    if(function == motorVelocity_)
+    {
+        status = m_pGCSController->setVelocityCts(pAxis, value);
+    }
+    else if(function == motorAccel_)
+    {
+        status = m_pGCSController->setAccelerationCts(pAxis, value);
+    }
+    else if (function == PI_SUP_TARGET)
     {
     	printf("PI_SUP_TargetAO: %f for axis %d\n", value, pAxis->axisNo_);
     }

diff --git a/pigcs2App/src/PIasynController.h b/pigcs2App/src/PIasynController.h
@@ -52,6 +52,7 @@ class PIasynController : asynMotorController {
     PIGCSController* m_pGCSController;
 
     int PI_SUP_POSITION;
+    int PI_SUP_POSITION_EGU;
     int PI_SUP_TARGET;
     int PI_SUP_SERVO;
     int PI_SUP_LAST_ERR;
@@ -66,6 +67,7 @@ class PIasynController : asynMotorController {
 
 
 #define PI_SUP_POSITION_String		"PI_SUP_POSITION"
+#define PI_SUP_POSITION_EGU_String      "PI_SUP_POSITION_EGU"
 #define PI_SUP_TARGET_String		"PI_SUP_TARGET"
 #define PI_SUP_SERVO_String			"PI_SUP_SERVO"
 #define PI_SUP_LAST_ERR_String		"PI_SUP_LAST_ERR"