forked from epics_driver_modules/motorBase
Steffen Rau's (PI) support for Physik Instrumente's (PI) General Command Set (GCS2).
This commit is contained in:
Ron Sluiter
@@ -0,0 +1,811 @@
|
||||
/*
|
||||
* PIGCSController.cpp
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include <asynOctetSyncIO.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include "PIGCSController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include "PIGCSMotorController.h"
|
||||
#include "PIGCSPiezoController.h"
|
||||
#include "PIE517Controller.h"
|
||||
#include "PIE755Controller.h"
|
||||
#include "PIHexapodController.h"
|
||||
#include "PIC702Controller.h"
|
||||
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
extern "C" {
|
||||
int TranslatePIError(const int error, char* szBuffer, const int maxlen);
|
||||
}
|
||||
|
||||
double PIGCSController::TIMEOUT = 5.0;
|
||||
|
||||
/**
|
||||
* create instance of GCS controller depending on identification (\a szIDN)
|
||||
*/
|
||||
PIGCSController* PIGCSController::CreateGCSController(asynUser* pInterface, const char* szIDN)
|
||||
{
|
||||
if ( strstr(szIDN, "C-663") != NULL
|
||||
|| strstr(szIDN, "C-863") != NULL
|
||||
|| strstr(szIDN, "C-867") != NULL
|
||||
)
|
||||
{
|
||||
return new PIGCSMotorController(pInterface, szIDN);
|
||||
}
|
||||
else if ( strstr(szIDN, "E-517") != NULL)
|
||||
{
|
||||
return new PIE517Controller(pInterface, szIDN);
|
||||
}
|
||||
else if ( strstr(szIDN, "E-755") != NULL)
|
||||
{
|
||||
return new PIE755Controller(pInterface, szIDN);
|
||||
}
|
||||
else if ( strstr(szIDN, "C-702") != NULL)
|
||||
{
|
||||
return new PIC702Controller(pInterface, szIDN);
|
||||
}
|
||||
else if ( strstr(szIDN, "HEXAPOD") != NULL
|
||||
|| strstr(szIDN, "F-HEX") != NULL
|
||||
|| strstr(szIDN, "F-206") != NULL
|
||||
|| strstr(szIDN, "M-8") != NULL
|
||||
|| strstr(szIDN, "C-887") != NULL
|
||||
)
|
||||
{
|
||||
return new PIHexapodController(pInterface, szIDN);
|
||||
}
|
||||
else
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendOnly(asynUser* pInterface, const char *outputBuff, asynUser* logSink)
|
||||
{
|
||||
size_t nRequested=strlen(outputBuff);
|
||||
size_t nActual;
|
||||
asynStatus status;
|
||||
|
||||
asynPrint(logSink, ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendOnly() sending \"%s\"\n", outputBuff);
|
||||
//printf("PIGCSController::sendOnly() sending \"%s\"\n", outputBuff);
|
||||
|
||||
status = pasynOctetSyncIO->write(pInterface, outputBuff,
|
||||
nRequested, TIMEOUT, &nActual);
|
||||
if (nActual != nRequested)
|
||||
status = asynError;
|
||||
status = pasynOctetSyncIO->write(pInterface, "\n",
|
||||
1, TIMEOUT, &nActual);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_ERROR|ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController:sendOnly: error sending command %s, sent=%d, status=%d\n",
|
||||
outputBuff, nActual, status);
|
||||
}
|
||||
return(status);
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendOnly(asynUser* pInterface, char c, asynUser* logSink)
|
||||
{
|
||||
size_t nActual;
|
||||
asynStatus status;
|
||||
|
||||
asynPrint(logSink, ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendOnly() sending \"#%d\"\n", int(c));
|
||||
//printf("PIGCSController::sendOnly() sending \"#%d\"\n", int(c));
|
||||
|
||||
status = pasynOctetSyncIO->write(pInterface, &c,
|
||||
1, TIMEOUT, &nActual);
|
||||
if (nActual != 1)
|
||||
status = asynError;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_ERROR|ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController:sendOnly: error sending command %d, sent=%d, status=%d\n",
|
||||
int(c), nActual, status);
|
||||
//printf("PIGCSController::sendOnly() sending \"#%d\"\n", int(c));
|
||||
}
|
||||
return(status);
|
||||
}
|
||||
|
||||
|
||||
asynStatus PIGCSController::sendAndReceive(asynUser* pInterface, const char *outputBuff, char *inputBuff, int inputSize, asynUser* logSink)
|
||||
{
|
||||
size_t nWriteRequested=strlen(outputBuff);
|
||||
size_t nWrite, nRead;
|
||||
int eomReason;
|
||||
asynStatus status;
|
||||
size_t pos = 0;
|
||||
asynPrint(logSink, ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendAndReceive() sending \"%s\"\n", outputBuff);
|
||||
// //printf("PIGCSController::sendAndReceive() sending \"%s\"\n", outputBuff);
|
||||
|
||||
status = pasynOctetSyncIO->write(pInterface, outputBuff,
|
||||
nWriteRequested, TIMEOUT, &nWrite);
|
||||
if (nWrite != nWriteRequested)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_ERROR|ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController:sendAndReceive error calling write, output=%s status=%d, error=%s\n",
|
||||
outputBuff, status, pInterface->errorMessage);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
status = pasynOctetSyncIO->writeRead(pInterface,
|
||||
"\n", 1,
|
||||
inputBuff, inputSize,
|
||||
TIMEOUT, &nWrite, &nRead, &eomReason);
|
||||
if (nWrite != 1)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_ERROR|ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController:sendAndReceive error calling write, output=%s status=%d, error=%s\n",
|
||||
outputBuff, status, pInterface->errorMessage);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_ERROR|ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController:sendAndReceive error calling writeRead, output=%s status=%d, error=%s\n",
|
||||
outputBuff, status, pInterface->errorMessage);
|
||||
}
|
||||
while(inputBuff[strlen(inputBuff)-1] == ' ')
|
||||
{
|
||||
inputBuff[strlen(inputBuff)] = '\n';
|
||||
pos += nRead + 1;
|
||||
status = pasynOctetSyncIO->read(logSink,
|
||||
inputBuff+pos, inputSize-pos,
|
||||
TIMEOUT, &nRead, &eomReason);
|
||||
|
||||
}
|
||||
asynPrint(logSink, ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendAndReceive() received \"%s\"\n", inputBuff);
|
||||
// //printf("PIGCSController::sendAndReceive() received \"%s\"\n", inputBuff);
|
||||
|
||||
return(status);
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendOnly(const char *outputBuff)
|
||||
{
|
||||
asynUser* logSink = m_pCurrentLogSink;
|
||||
if (NULL == logSink)
|
||||
logSink = m_pInterface;
|
||||
m_interfaceMutex.lock();
|
||||
asynStatus status = sendOnly(m_pInterface, outputBuff, logSink);
|
||||
m_interfaceMutex.unlock();
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendOnly(char c)
|
||||
{
|
||||
asynUser* logSink = m_pCurrentLogSink;
|
||||
if (NULL == logSink)
|
||||
logSink = m_pInterface;
|
||||
m_interfaceMutex.lock();
|
||||
asynStatus status = sendOnly(m_pInterface, c, logSink);
|
||||
m_interfaceMutex.unlock();
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendAndReceive(char c, char *inputBuff, int inputSize)
|
||||
{
|
||||
asynUser* logSink = m_pCurrentLogSink;
|
||||
if (NULL == logSink)
|
||||
logSink = m_pInterface;
|
||||
m_interfaceMutex.lock();
|
||||
asynStatus status = sendAndReceive(m_pInterface, c, inputBuff, inputSize, logSink);
|
||||
m_interfaceMutex.unlock();
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendAndReceive(const char* output, char *inputBuff, int inputSize)
|
||||
{
|
||||
asynUser* logSink = m_pCurrentLogSink;
|
||||
if (NULL == logSink)
|
||||
logSink = m_pInterface;
|
||||
m_interfaceMutex.lock();
|
||||
asynStatus status = sendAndReceive(m_pInterface, output, inputBuff, inputSize, logSink);
|
||||
m_interfaceMutex.unlock();
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::sendAndReceive(asynUser* pInterface, char c, char *inputBuff, int inputSize, asynUser* logSink)
|
||||
{
|
||||
size_t nWrite, nRead;
|
||||
int eomReason;
|
||||
asynStatus status;
|
||||
size_t pos = 0;
|
||||
|
||||
asynPrint(logSink, ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendAndReceive() sending \"#%d\"\n", int(c));
|
||||
//printf("PIGCSController::sendAndReceive() sending \"#%d\"\n", int(c));
|
||||
status = pasynOctetSyncIO->writeRead(pInterface,
|
||||
&c, 1,
|
||||
inputBuff, inputSize,
|
||||
TIMEOUT, &nWrite, &nRead, &eomReason);
|
||||
if (nWrite != 1)
|
||||
status = asynError;
|
||||
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_ERROR|ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendAndReceive error calling writeRead, output=%d status=%d, error=%s\n",
|
||||
int(c), status, pInterface->errorMessage);
|
||||
//printf("PIGCSController::sendAndReceive error calling writeRead, output=%d status=%d, error=%s\n", int(c), status, pInterface->errorMessage);
|
||||
}
|
||||
|
||||
while(inputBuff[strlen(inputBuff)-1] == ' ')
|
||||
{
|
||||
pos += nRead;
|
||||
status = pasynOctetSyncIO->writeRead(logSink,
|
||||
&c, 1,
|
||||
inputBuff+pos, inputSize-pos,
|
||||
TIMEOUT, &nWrite, &nRead, &eomReason);
|
||||
//printf("PIGCSController::sendAndReceive(char) in while loop. inputBuff: \"%s\"\n", inputBuff);
|
||||
}
|
||||
asynPrint(logSink, ASYN_TRACEIO_DRIVER,
|
||||
"PIGCSController::sendAndReceive() received \"%s\"\n", inputBuff);
|
||||
//printf("PIGCSController::sendAndReceive() received \"%s\" - (0x%02X)\n", inputBuff, int(inputBuff[0]));
|
||||
|
||||
return(status);
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::setVelocityCts( PIasynAxis* pAxis, double velocity )
|
||||
{
|
||||
char cmd[100];
|
||||
velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(cmd,"VEL %s %f", pAxis->m_szAxisName, velocity);
|
||||
asynStatus status = sendOnly(cmd);
|
||||
if (asynSuccess == status)
|
||||
{
|
||||
pAxis->m_velocity = velocity;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
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 = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode == 0)
|
||||
return asynSuccess;
|
||||
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIGCSController::moveCts(array) failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
|
||||
|
||||
asynStatus PIGCSController::moveCts( PIasynAxis* pAxis, int targetCts )
|
||||
{
|
||||
asynStatus status;
|
||||
char cmd[100];
|
||||
double target = double(targetCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIGCSController::moveCts(, %d) \n", targetCts);
|
||||
return move(pAxis, target);
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::move( PIasynAxis* pAxis, double target )
|
||||
{
|
||||
asynStatus status;
|
||||
char cmd[100];
|
||||
sprintf(cmd,"MOV %s %f", pAxis->m_szAxisName, target);
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
asynPrint(m_pInterface, 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, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIGCSController::move() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int PIGCSController::getGCSError()
|
||||
{
|
||||
char buf[256];
|
||||
asynStatus status = sendAndReceive("ERR?", buf, 255);
|
||||
if (asynTimeout == status)
|
||||
{
|
||||
return COM_TIMEOUT;
|
||||
}
|
||||
else if (asynSuccess != status)
|
||||
{
|
||||
return COM_ERROR;
|
||||
}
|
||||
int errorCode = atoi(buf);
|
||||
if (0 != errorCode)
|
||||
{
|
||||
m_LastError = errorCode;
|
||||
asynPrint(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_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"PIGCSController::getGCSError() GCS error, %s\n",
|
||||
szErrorMsg);
|
||||
|
||||
}
|
||||
}
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::haltAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
char cmd[100];
|
||||
sprintf(cmd,"HLT %s", pAxis->m_szAxisName);
|
||||
asynStatus status = 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_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIGCSController::haltAxis() failed, GCS error %d", err);
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* get position of axis in physical units (EGU) as defined on the controller
|
||||
*/
|
||||
asynStatus PIGCSController::getAxisPosition(PIasynAxis* pAxis, double& position)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "POS? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (!getValue(buf, position))
|
||||
{
|
||||
status = asynError;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* get velocity of axis in physical units (EGU) as defined on the controller
|
||||
* and set PIasynAxis::m_velocity
|
||||
*/
|
||||
asynStatus PIGCSController::getAxisVelocity(PIasynAxis* pAxis)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "VEL? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (!getValue(buf, pAxis->m_velocity))
|
||||
{
|
||||
status = asynError;
|
||||
}
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Find travel range for axis.
|
||||
*/
|
||||
asynStatus PIGCSController::getTravelLimits(PIasynAxis* pAxis, double& negLimit, double& posLimit)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "TMN? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (!getValue(buf, negLimit))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
sprintf(cmd, "TMX? %s", pAxis->m_szAxisName);
|
||||
status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (!getValue(buf, posLimit))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::hasLimitSwitches(PIasynAxis* pAxis)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "LIM? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = 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 = 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_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::hasLimitSwitches() axis has %slimit switches\n",
|
||||
pAxis->m_bHasLimitSwitches?"":"no ");
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::hasReferenceSensor(PIasynAxis* pAxis)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "TRS? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
if (!getValue(buf, pAxis->m_bHasReference))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::hasReferenceSwitch() axis has %sreference sensor\n",
|
||||
pAxis->m_bHasReference?"":"no ");
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* get position of axis in counts as used in EPICS.
|
||||
* getAxisPosition() is called and position is covnerted to counts using the
|
||||
* counts-per-unit (CPU) fraction of the axis.
|
||||
*/
|
||||
asynStatus PIGCSController::getAxisPositionCts(PIasynAxis* pAxis)
|
||||
{
|
||||
double pos;
|
||||
asynStatus status = getAxisPosition(pAxis, pos);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_position = pos;
|
||||
if (pAxis->m_CPUdenominator==0 || pAxis->m_CPUnumerator==0)
|
||||
{
|
||||
pAxis->m_positionCts = pos;
|
||||
return status;
|
||||
}
|
||||
|
||||
pAxis->m_positionCts = int( (pos * double(pAxis->m_CPUnumerator) / double(pAxis->m_CPUdenominator))+0.5);
|
||||
if (m_pCurrentLogSink != NULL)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::getAxisPositionCts() pos:%d\n",
|
||||
pAxis->m_positionCts);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
//void PIGCSController::calcAxisPositionCts();
|
||||
|
||||
asynStatus PIGCSController::setServo(PIasynAxis* pAxis, int servoState)
|
||||
{
|
||||
char cmd[100];
|
||||
sprintf(cmd, "SVO %s %d", pAxis->m_szAxisName, servoState);
|
||||
asynStatus status = 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_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"Could not set servo state!\n");
|
||||
return asynError;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::getMoving(PIasynAxis* pAxis, int& moving)
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(5), buf, 99);;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
//printf("PIGCSController::getMoving() failed, status %d", status);
|
||||
return status;
|
||||
}
|
||||
|
||||
char* pStr;
|
||||
long movingState = strtol(buf, &pStr, 16);
|
||||
moving = (movingState & pAxis->m_movingStateMask) != 0 ? 1 : 0;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::getBusy(PIasynAxis* pAxis, int& busy)
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(7), buf, 99);;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
unsigned char c = (unsigned char)buf[0];
|
||||
busy = (c==0xB0);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::setGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double value)
|
||||
{
|
||||
char cmd[100];
|
||||
sprintf(cmd, "SPA %s %d %.12g", pAxis->m_szAxisName, paramID, value);
|
||||
asynStatus status = sendOnly(cmd);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::getGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double& value)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "SPA? %s %d", pAxis->m_szAxisName, paramID);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
if (!getValue(buf, value))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
PIGCSController::PIGCSController(asynUser* pCom, const char* szIDN)
|
||||
: m_pCurrentLogSink(NULL)
|
||||
, m_bAnyAxisMoving(false)
|
||||
, m_pInterface(pCom)
|
||||
, m_nrFoundAxes(0)
|
||||
, m_LastError(0)
|
||||
{
|
||||
strncpy(szIdentification, szIDN, 199);
|
||||
}
|
||||
|
||||
PIGCSController::~PIGCSController()
|
||||
{
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::initAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
// read stage name - to have it in logfile and find
|
||||
// problems because of mis/non-configured controllers
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "CST? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::initAxis() stage configuration: %s\n", buf);
|
||||
}
|
||||
pAxis->m_movingStateMask = pow(2.0, pAxis->getAxisNo());
|
||||
|
||||
return setServo(pAxis, 1);
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::init(void)
|
||||
{
|
||||
asynStatus status;
|
||||
status = findConnectedAxes();
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::findConnectedAxes()
|
||||
{
|
||||
m_nrFoundAxes = 0;
|
||||
for (size_t i=0; i<MAX_NR_AXES; i++)
|
||||
{
|
||||
m_axesIDs[i] = NULL;
|
||||
}
|
||||
asynStatus status = PIGCSController::sendAndReceive("SAI?", m_allAxesIDs, 255);
|
||||
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
char* szAxis = strtok(m_allAxesIDs, "\n");
|
||||
while (szAxis != NULL)
|
||||
{
|
||||
int i=strlen(szAxis)-1;
|
||||
while (szAxis[i] == ' ')
|
||||
{
|
||||
szAxis[i] = '\0';
|
||||
i--;
|
||||
}
|
||||
if (MAX_NR_AXES <= m_nrFoundAxes)
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
m_axesIDs[m_nrFoundAxes] = szAxis;
|
||||
m_nrFoundAxes++;
|
||||
szAxis = strtok(NULL, "\n");
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "FRF? %s", pAxis->m_szAxisName);
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (getValue(buf, pAxis->m_homed))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::getResolution(PIasynAxis* pAxis, double& resolution )
|
||||
{
|
||||
resolution = 0.0001;
|
||||
pAxis->m_CPUnumerator = 10000;
|
||||
pAxis->m_CPUdenominator = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::SetPivotX(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::SetPivotX() ignored");
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::SetPivotY(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::SetPivotY() ignored");
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIGCSController::SetPivotZ(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::SetPivotZ() ignored");
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
bool PIGCSController::getValue(const char* szMsg, double& value)
|
||||
{
|
||||
const char* p = strstr(szMsg, "=");
|
||||
if (p==NULL || *p == '\0')
|
||||
{
|
||||
return false;
|
||||
}
|
||||
value = atof(p+1);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PIGCSController::getValue(const char* szMsg, int& value)
|
||||
{
|
||||
const char* p = strstr(szMsg, "=");
|
||||
if (p==NULL || *p == '\0')
|
||||
{
|
||||
return false;
|
||||
}
|
||||
value = atoi(p+1);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PIGCSController::getValue(const char* szMsg, bool& value)
|
||||
{
|
||||
const char* p = strstr(szMsg, "=");
|
||||
if (p==NULL || *p == '\0')
|
||||
{
|
||||
return false;
|
||||
}
|
||||
int ivalue = atoi(p+1);
|
||||
value = (ivalue =! 0);
|
||||
return true;
|
||||
}
|
||||
Reference in New Issue
Block a user