static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int value)
{
int ret_status = MOTOR_AXIS_ERROR;
int status;
char buff[100];
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
epicsMutexLock(pAxis->mutexId);
switch (function)
{
case motorAxisClosedLoop:
/* The MM4000 only allows turning on and off ALL motors (MO and MF commands), */
/* not individual axes. Don't implement */
if (pAxis->pController->model == MM4000)
ret_status = MOTOR_AXIS_OK;
else
{
if (value == 0)
{
int axisStatus, powerOn = 0;
int offset = (pAxis->axis * 5) + 3; /* Offset in status string */
sprintf(buff, "%dMF", pAxis->axis + 1);
ret_status = sendOnly(pAxis->pController, buff);
/* Wait for Power to come on. */
while (powerOn == 0)
{
ret_status = sendAndReceive(pAxis->pController, "MS;", buff, sizeof(buff));
axisStatus = buff[offset];
if (!(axisStatus & MM4000_POWER_OFF))
powerOn = 1;
else
epicsThreadSleep(0.1);
}
}
else
{
sprintf(buff, "%dMO", pAxis->axis+1);
ret_status = sendOnly(pAxis->pController, buff);
}
}
break;
default:
PRINT(pAxis->logParam, MOTOR_ERROR, "motorAxisSetInteger: unknown function %d\n", function);
break;
}
if (ret_status != MOTOR_AXIS_ERROR)
{
status = motorParam->setInteger(pAxis->params, function, value);
motorParam->callCallback(pAxis->params);
}
epicsMutexUnlock(pAxis->mutexId);
return(ret_status);
}
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);
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_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 = sendOnly(cmd);
if (asynSuccess != status)
return status;
int errorCode = getGCSError();
if (errorCode == 0)
{
return asynSuccess;
}
asynPrint(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;
}
static int motorAxisHome(AXIS_HDL pAxis, double min_velocity, double max_velocity, double acceleration, int forwards)
{
int status;
char buff[100];
if (pAxis == NULL)
return MOTOR_AXIS_ERROR;
PRINT(pAxis->logParam, FLOW, "motorAxisHome: set card %d, axis %d to home\n",
pAxis->card, pAxis->axis);
sprintf(buff, "%dAC%.*f; %dVA%.*f;%dOR;",
pAxis->axis+1, pAxis->maxDigits, acceleration * pAxis->stepSize,
pAxis->axis+1, pAxis->maxDigits, max_velocity * pAxis->stepSize,
pAxis->axis+1);
status = sendOnly(pAxis->pController, buff);
if (status)
return(MOTOR_AXIS_ERROR);
if (epicsMutexLock(pAxis->mutexId) == epicsMutexLockOK)
{
/* Insure that the motor record's next status update sees motorAxisDone = False. */
motorParam->setInteger(pAxis->params, motorAxisDone, 0);
motorParam->callCallback(pAxis->params);
epicsMutexUnlock(pAxis->mutexId);
}
/* Send a signal to the poller task which will make it do a poll, and switch to the moving poll rate */
epicsEventSignal(pAxis->pController->pollEventId);
return MOTOR_AXIS_OK;
}
static int motorAxisStop(AXIS_HDL pAxis, double acceleration)
{
int status;
char buff[100];
if (pAxis == NULL)
return MOTOR_AXIS_ERROR;
PRINT(pAxis->logParam, FLOW, "Set card %d, axis %d to stop with accel=%f\n",
pAxis->card, pAxis->axis, acceleration);
sprintf(buff, "%dAC%.*f;%dST;",
pAxis->axis+1, pAxis->maxDigits, acceleration * pAxis->stepSize,
pAxis->axis+1);
status = sendOnly(pAxis->pController, buff);
if (status)
return MOTOR_AXIS_ERROR;
return MOTOR_AXIS_OK;
}
static int motorAxisMove(AXIS_HDL pAxis, double position, int relative,
double min_velocity, double max_velocity, double acceleration)
{
int status;
char buff[100];
char *moveCommand;
if (pAxis == NULL)
return MOTOR_AXIS_ERROR;
PRINT(pAxis->logParam, FLOW, "Set card %d, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f\n",
pAxis->card, pAxis->axis, position, min_velocity, max_velocity, acceleration);
if (relative) {
moveCommand = "PR";
} else {
moveCommand = "PA";
}
sprintf(buff, "%dAC%.*f;%dVA%.*f;%d%s%.*f;",
pAxis->axis+1, pAxis->maxDigits, acceleration * pAxis->stepSize,
pAxis->axis+1, pAxis->maxDigits, max_velocity * pAxis->stepSize,
pAxis->axis+1, moveCommand, pAxis->maxDigits, position * pAxis->stepSize);
status = sendOnly(pAxis->pController, buff);
if (status)
return MOTOR_AXIS_ERROR;
if (epicsMutexLock(pAxis->mutexId) == epicsMutexLockOK)
{
/* Insure that the motor record's next status update sees motorAxisDone = False. */
motorParam->setInteger(pAxis->params, motorAxisDone, 0);
motorParam->callCallback(pAxis->params);
epicsMutexUnlock(pAxis->mutexId);
}
/* Send a signal to the poller task which will make it do a poll, and switch to the moving poll rate */
epicsEventSignal(pAxis->pController->pollEventId);
return MOTOR_AXIS_OK;
}
static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double value)
{
int ret_status = MOTOR_AXIS_ERROR;
double deviceValue;
char buff[100];
if (pAxis == NULL)
return MOTOR_AXIS_ERROR;
else
{
epicsMutexLock(pAxis->mutexId);
switch (function)
{
case motorAxisPosition:
{
deviceValue = value*pAxis->stepSize;
sprintf(buff, "%dSH%.*f;%dDH;%dSH%.*f", pAxis->axis+1, pAxis->maxDigits, deviceValue,
pAxis->axis+1, pAxis->axis+1, pAxis->maxDigits, pAxis->homePreset);
ret_status = sendOnly(pAxis->pController, buff);
break;
}
case motorAxisEncoderRatio:
{
PRINT(pAxis->logParam, MOTOR_ERROR, "motorAxisSetDouble: MM4000 does not support setting encoder ratio\n");
break;
}
case motorAxisResolution:
{
PRINT(pAxis->logParam, MOTOR_ERROR, "motorAxisSetDouble: MM4000 does not support setting resolution\n");
break;
}
case motorAxisLowLimit:
{
deviceValue = value*pAxis->stepSize;
sprintf(buff, "%dSL%.*f", pAxis->axis+1, pAxis->maxDigits, deviceValue);
ret_status = sendOnly(pAxis->pController, buff);
break;
}
case motorAxisHighLimit:
{
deviceValue = value*pAxis->stepSize;
sprintf(buff, "%dSR%.*f", pAxis->axis+1, pAxis->maxDigits, deviceValue);
ret_status = sendOnly(pAxis->pController, buff);
break;
}
case motorAxisPGain:
{
PRINT(pAxis->logParam, MOTOR_ERROR, "MM4000 does not support setting proportional gain\n");
break;
}
case motorAxisIGain:
{
PRINT(pAxis->logParam, MOTOR_ERROR, "MM4000 does not support setting integral gain\n");
break;
}
case motorAxisDGain:
{
PRINT(pAxis->logParam, MOTOR_ERROR, "MM4000 does not support setting derivative gain\n");
break;
}
default:
PRINT(pAxis->logParam, MOTOR_ERROR, "motorAxisSetDouble: unknown function %d\n", function);
break;
}
if (ret_status == MOTOR_AXIS_OK )
{
motorParam->setDouble(pAxis->params, function, value);
motorParam->callCallback(pAxis->params);
}
epicsMutexUnlock(pAxis->mutexId);
}
return ret_status;
}
/**
* read just one line of input
*/
asynStatus omsMAXv::sendReceive(const char *outputBuff, char *inputBuff, unsigned int inputSize)
{
static const char* functionName = "sendReceive";
STATUS1 flag1;
epicsUInt16 getIndex, putIndex;
size_t bufsize;
size_t usedSpace = 0;
char *start, *end;
int itera = 0;
asynStatus status;
if (!enabled) return asynError;
status = sendOnly(outputBuff);
if (status != asynSuccess) return status;
if (inputSize <= 0) return status;
*inputBuff = '\0';
double time = 0.0;
double timeout = 0.1;
// skip busy-waiting for small epicsThreadSleepQuantum
if (epicsThreadSleepQuantum() <= 0.01) itera = 2001;
while ((pmotor->status1_flag.Bits.text_response == 0) && (time < timeout)){
Debug(32, "%s:%s:%s: Waiting for reponse, itera:%d\n",
driverName, functionName, portName, itera);
// busy-waiting but not more than 2000 times
if (itera > 2000){
time += epicsThreadSleepQuantum();
epicsThreadSleep(epicsThreadSleepQuantum());
}
itera++;
}
if (pmotor->status1_flag.Bits.text_response == 0)
{
Debug(1, "%s:%s:%s: Timeout occurred , %s\n",
driverName, functionName, portName, outputBuff);
return asynTimeout;
}
getIndex = (epicsUInt16) pmotor->inGetIndex;
putIndex = (epicsUInt16) pmotor->inPutIndex;
bufsize = putIndex - getIndex;
start = (char *) &pmotor->inBuffer[getIndex];
end = (char *) &pmotor->inBuffer[putIndex];
if (start < end) { /* Test for message wraparound in buffer. */
usedSpace = MIN(bufsize, inputSize);
memcpy(inputBuff, start, usedSpace);
}
else
{
bufsize += BUFFER_SIZE;
size_t firstPart = ((char *) &pmotor->inBuffer[BUFFER_SIZE]) - start;
usedSpace = MIN(firstPart, inputSize);
memcpy(inputBuff, start, usedSpace);
size_t copySize = MIN(bufsize - firstPart, inputSize - usedSpace);
memcpy((inputBuff + usedSpace), (const char *) &pmotor->inBuffer[0], copySize);
usedSpace += copySize;
}
inputBuff[usedSpace - 1]= '\0';
getIndex += bufsize;
if (getIndex >= BUFFER_SIZE)
getIndex -= BUFFER_SIZE;
while (getIndex != (epicsUInt16)pmotor->inPutIndex)
{
Debug(2, "readbuf(): flushed - %d\n", pmotor->inBuffer[getIndex]);
if (++getIndex > BUFFER_SIZE)
getIndex = 0;
}
pmotor->status1_flag.Bits.text_response = 0;
pmotor->inGetIndex = (epicsUInt32) getIndex;
flag1.All = pmotor->status1_flag.All;
pmotor->status1_flag.All = flag1.All;
pmotor->msg_semaphore=0;
Debug(16, "omsMAXv::sendReceive: received %s\n", inputBuff);
return asynSuccess;
}
/**
* Just send and log but do not check sent log
*
* @param RR Runtime environment
* @param toAddr Destination address
*/
inline void sendAndLog(const RuntimeEnvironment *RR,const Address &toAddr)
{
_alreadySentTo.push_back(toAddr);
sendOnly(RR,toAddr);
}
