diff --git a/pigcs2App/Db/Makefile b/pigcs2App/Db/Makefile index 201f21f..1bd4e8b 100644 --- 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 .db template is not named *.template add diff --git a/pigcs2App/Db/PI_PIGCSasynController.db b/pigcs2App/Db/PI_PIGCSasynController.db new file mode 100644 index 0000000..38382bd --- /dev/null +++ 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") +} \ No newline at end of file diff --git a/pigcs2App/src/PIGCSController.cpp b/pigcs2App/src/PIGCSController.cpp index 1184c0a..17df8ea 100644 --- 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 index 29db5fb..55e7254 100644 --- 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 index 9539e89..524b9d9 100644 --- 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 index ca9b0e1..a924b4e 100644 --- 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 index a6961b9..8b2b05c 100644 --- 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"