asynStatus PIGCSController::moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes)
{
//TODO: "Use MVE or set vel/acc so axes reach target at the same time""
asynStatus status;
char cmd[1000] = "MOV";
char subCmd[100];
for (int axis = 0; axis <numAxes; axis++)
{
PIasynAxis* pAxis = pAxesArray[axis];
double target = double(pTargetCtsArray[axis]) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
sprintf(subCmd," %s %f", pAxis->m_szAxisName, target);
strcat(cmd, subCmd);
pAxis->m_lastDirection = (pTargetCtsArray[axis] > pAxis->m_positionCts) ? 1 : 0;
}
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_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::moveCts(array) failed, GCS error %d\n", errorCode);
return asynError;
}
asynStatus PIGCSMotorController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
{
asynStatus status = setServo(pAxis, 1);
if (asynSuccess != status)
return status;
char cmd[100];
if (velocity != 0)
{
velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
sprintf(cmd,"SPA %s 0x50 %f", pAxis->m_szAxisName, velocity);
m_pInterface->sendOnly(cmd);
}
if (pAxis->m_bHasReference)
{
// call FRF - find reference
sprintf(cmd,"FRF %s", pAxis->m_szAxisName);
}
else if (pAxis->m_bHasLimitSwitches)
{
if (forwards)
{
// call FPL - find positive limit switch
sprintf(cmd,"FPL %s", pAxis->m_szAxisName);
}
else
{
// call FNL - find negative limit switch
sprintf(cmd,"FNL %s", pAxis->m_szAxisName);
}
}
else
{
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR,
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit 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,
"PIGCSMotorController::referenceVelCts() failed\n");
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
"PIGCSMotorController::referenceVelCts() failed - GCS Error %d\n",errorCode);
return asynError;
}
asynStatus PIGCSController::hasLimitSwitches(PIasynAxis* pAxis)
{
char cmd[100];
char buf[255];
sprintf(cmd, "LIM? %s", pAxis->m_szAxisName);
asynStatus status = m_pInterface->sendAndReceive(cmd, buf, 99);
if (status != asynSuccess)
{
return status;
}
if (!getValue(buf, pAxis->m_bHasLimitSwitches))
{
return asynError;
}
if (!pAxis->m_bHasLimitSwitches)
{
sprintf(cmd, "HAR? %s", pAxis->m_szAxisName);
asynStatus status = m_pInterface->sendAndReceive(cmd, buf, 99);
if (status == asynSuccess)
{
if (!getValue(buf, pAxis->m_bHasLimitSwitches))
{
return asynError;
}
}
else if (status == asynTimeout)
{
int err = getGCSError();
if (err == PI_CNTR_UNKNOWN_COMMAND)
{
// "HAR?" not known
pAxis->m_bHasLimitSwitches = false;
}
else
{
return status;
}
}
else
{
return status;
}
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW,
"PIGCSController::hasLimitSwitches() axis has %slimit switches\n",
pAxis->m_bHasLimitSwitches?"":"no ");
return status;
}
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;
}
asynStatus PIGCSController::haltAxis(PIasynAxis* pAxis)
{
char cmd[100];
sprintf(cmd,"HLT %s", pAxis->m_szAxisName);
asynStatus status = m_pInterface->sendOnly(cmd);
if (status != asynSuccess)
{
return status;
}
int err = getGCSError();
// controller will set error code to PI_CNTR_STOP (10)
if (err != PI_CNTR_STOP)
{
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::haltAxis() failed, GCS error %d", err);
return asynError;
}
return status;
}
asynStatus PIGCSController::move( PIasynAxis* pAxis, double target )
{
asynStatus status;
char cmd[100];
sprintf(cmd,"MOV %s %f", pAxis->m_szAxisName, target);
status = m_pInterface->sendOnly(cmd);
if (asynSuccess != status)
{
return status;
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::move() sent \"%s\"\n", cmd);
pAxis->m_lastDirection = (target > pAxis->m_position) ? 1 : 0;
int errorCode = getGCSError();
if (errorCode == 0)
return asynSuccess;
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::move() failed, GCS error %d\n", errorCode);
return asynError;
}
asynStatus PIGCSController::setAxisPosition(PIasynAxis* pAxis, double position)
{
asynStatus status;
char cmd[100];
sprintf(cmd,"RON %s 0", pAxis->m_szAxisName);
status = m_pInterface->sendOnly(cmd);
if (asynSuccess != status)
{
return status;
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::setAxisPosition() sent \"%s\"\n", cmd);
sprintf(cmd,"POS %s %f", pAxis->m_szAxisName, position);
status = m_pInterface->sendOnly(cmd);
if (asynSuccess != status)
{
return status;
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::setAxisPosition() sent \"%s\"\n", cmd);
sprintf(cmd,"RON %s 1", pAxis->m_szAxisName);
status = m_pInterface->sendOnly(cmd);
if (asynSuccess != status)
{
return status;
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::setAxisPosition() sent \"%s\"\n", cmd);
int errorCode = getGCSError();
if (errorCode == 0)
return asynSuccess;
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
"PIGCSController::setAxisPosition() failed, GCS error %d\n", errorCode);
return asynError;
}
asynStatus PIGCSController::setServo(PIasynAxis* pAxis, int servoState)
{
char cmd[100];
sprintf(cmd, "SVO %s %d", pAxis->m_szAxisName, servoState);
asynStatus status = m_pInterface->sendOnly(cmd);
if (status != asynSuccess)
{
return status;
}
int err = getGCSError();
if (COM_NO_ERROR == err)
{
pAxis->m_bServoControl = (servoState == 1);
if (pAxis->m_bProblem && pAxis->m_bServoControl)
{
pAxis->m_bProblem = false;
}
return asynSuccess;
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
"Could not set servo state!\n");
return asynError;
}
