diff --git a/motorApp/PIGCS2Src/PIGCSController.cpp b/motorApp/PIGCS2Src/PIGCSController.cpp index 592934f9..1184c0ab 100644 --- a/motorApp/PIGCS2Src/PIGCSController.cpp +++ b/motorApp/PIGCS2Src/PIGCSController.cpp @@ -113,6 +113,10 @@ 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); @@ -249,17 +253,21 @@ int PIGCSController::getGCSError() if (0 != errorCode) { m_LastError = errorCode; - asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW, - "PIGCSController::getGCSError() GCS error code = %d\n", - errorCode); - char szErrorMsg[1024]; - if (TranslatePIError(errorCode, szErrorMsg, 1024)) - { - asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW, - "PIGCSController::getGCSError() GCS error, %s\n", - szErrorMsg); + if (m_pInterface->m_pCurrentLogSink) + { + asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW, + "PIGCSController::getGCSError() GCS error code = %d\n", + errorCode); + + char szErrorMsg[1024]; + if (TranslatePIError(errorCode, szErrorMsg, 1024)) + { + asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW, + "PIGCSController::getGCSError() GCS error, %s\n", + szErrorMsg); - } + } + } } return errorCode; } @@ -310,6 +318,10 @@ asynStatus PIGCSController::getAxisPosition(PIasynAxis* pAxis, double& position) */ asynStatus PIGCSController::getAxisVelocity(PIasynAxis* pAxis) { + if (!m_KnowsVELcommand) + { + return asynSuccess; + } char cmd[100]; char buf[255]; sprintf(cmd, "VEL? %s", pAxis->m_szAxisName); @@ -445,7 +457,7 @@ asynStatus PIGCSController::getAxisPositionCts(PIasynAxis* pAxis) pAxis->m_position = pos; if (pAxis->m_CPUdenominator==0 || pAxis->m_CPUnumerator==0) { - pAxis->m_positionCts = pos; + pAxis->m_positionCts = int(pos); return status; } @@ -579,7 +591,7 @@ asynStatus PIGCSController::initAxis(PIasynAxis* pAxis) asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW, "PIGCSController::initAxis() stage configuration: %s\n", buf); } - pAxis->m_movingStateMask = pow(2.0, pAxis->getAxisNo()); + pAxis->m_movingStateMask = int (pow(2.0, pAxis->getAxisNo()) ); return setServo(pAxis, 1); } @@ -587,6 +599,15 @@ asynStatus PIGCSController::initAxis(PIasynAxis* pAxis) asynStatus PIGCSController::init(void) { asynStatus status; + char buffer [1024]; + status = m_pInterface->sendAndReceive("VEL?", buffer, 1023); + m_KnowsVELcommand = ( asynSuccess == status); + if (!m_KnowsVELcommand) + { + (void) getGCSError (); + } + + status = findConnectedAxes(); return status; } diff --git a/motorApp/PIGCS2Src/PIGCSController.h b/motorApp/PIGCS2Src/PIGCSController.h index 722eedc9..29db5fb8 100644 --- a/motorApp/PIGCS2Src/PIGCSController.h +++ b/motorApp/PIGCS2Src/PIGCSController.h @@ -109,6 +109,8 @@ protected: size_t m_nrFoundAxes; char m_allAxesIDs[255]; int m_LastError; + + bool m_KnowsVELcommand; }; #endif /* PIGCSCONTROLLER_H_ */