lin-epics-modules/sinqepicsapp
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added AMOR detector tower special pmac motors: working now

Browse Source
This commit is contained in:
koennecke
2023-07-07 13:54:21 +02:00
parent 8a6441927a
commit f1a17bc295
5 changed files with 396 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
sinqEPICSApp/src/NanotecDriver.cpp
View File

@ -394,7 +394,12 @@ asynStatus NanotecAxis::poll(bool *moving)
pPtr = strchr(reply,'C');
pPtr++;
posVal = atoi(pPtr);
if(pPtr){
posVal = atoi(pPtr);
} else {
errlogPrintf("Invalid response %s for #C received for axis %d\n", reply, axisNo_);
return asynError;
}
//errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal);
setDoubleParam(pC_->motorPosition_, (double)posVal);
@ -408,7 +413,12 @@ asynStatus NanotecAxis::poll(bool *moving)
pPtr = strchr(reply,'$');
pPtr++;
statVal = atoi(pPtr);
if(pPtr) {
statVal = atoi(pPtr);
} else {
errlogPrintf("Invalid response %s for #$ received for axis %d\n", reply, axisNo_);
return asynError;
}
//errlogPrintf("Axis %d, reply %s, statVal = %d\n",
// axisNo_, reply, statVal);

313
sinqEPICSApp/src/pmacAxis.cpp
View File

@ -1,4 +1,4 @@
/********************************************
/*******************************************
* pmacAxis.cpp
*
* PMAC Asyn motor based on the
@ -28,6 +28,10 @@
*
* Michele Brambilla, February 2022
*
* Added driver for special AMOR detector axes
*
* Mark Koennecke, June 2023
*
********************************************/
#include <epicsExit.h>
@ -42,6 +46,7 @@
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include<algorithm>
#include<cmath>
@ -1181,3 +1186,309 @@ asynStatus pmacV3Axis::move(double position, int relative, double min_velocity,
updateMsgTxtFromDriver("");
return pmacAxis::move(position, relative, min_velocity, max_velocity, acceleration);
}
/*========================= Amor Detector Motor code ==============*/
AmorDetectorAxis::AmorDetectorAxis(pmacController *pC, int axisNo, int function)
: pmacAxis(pC, axisNo, false) {
static const char *functionName = "pmacAxis::pmacAxis";
pC_->debugFlow(functionName);
_function = function;
callParamCallbacks();
/* Wake up the poller task which will make it do a poll,
* updating values for this axis to use the new resolution (stepSize_) */
pC_->wakeupPoller();
}
/*-----------------------------------------------------------------*/
asynStatus AmorDetectorAxis::moveVelocity(double min_velocity,
double max_velocity,
double acceleration)
{
updateMsgTxtFromDriver("Function moveVelocity not allowed");
return asynError;
}
/*------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::home(double min_velocity,
double max_velocity,
double acceleration,
int forwards)
{
updateMsgTxtFromDriver("Cannot home AMOR detector motors");
return asynError;
}
/*-------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::setPosition(double position)
{
updateMsgTxtFromDriver("Cannot setPosition() AMOR detector motors");
return asynError;
}
/*-------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::move(double position, int relative,
double min_velocity, double max_velocity,
double acceleration)
{
asynStatus status = asynError;
static const char *functionName = "AmorDetectorAxis::move";
double realPosition;
int parkStatus;
time_t errorWait;
updateMsgTxtFromDriver("");
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
pC_->debugFlow(functionName);
/* test if we are not in the change position */
sprintf(command,"P358");
pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "%d", &parkStatus);
if(_function != ADPARK && parkStatus == 2) {
updateMsgTxtFromDriver("Cannot run motor when in park position");
errlogPrintf("Cannot run motor when in park position");
return asynError;
}
if (relative) {
realPosition = previous_position_ + position / MULT;
} else {
realPosition = position / MULT;
}
/* set target */
errlogPrintf("function %d, sending position %f\n", _function, realPosition);
switch(_function){
case ADCOM:
sprintf(command, "Q451=%f", realPosition);
break;
case ADCOZ:
sprintf(command, "Q454=%f", realPosition);
break;
case ADPARK:
// test if a reset is required first
sprintf(command, "P359");
pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "%d", &parkStatus);
errlogPrintf("park status %d\n", parkStatus);
if(parkStatus != 0){
sprintf(command,"P352=2");
pC_->lowLevelWriteRead(axisNo_, command, response);
errlogPrintf("Park status %d, error reset: %s\n", parkStatus, command);
}
errorWait = time(NULL);
while(true){
usleep(5000);
sprintf(command, "P359");
pC_->lowLevelWriteRead(axisNo_, command, response);
sscanf(response, "%d", &parkStatus);
errlogPrintf("P359 = %d\n", parkStatus);
if(parkStatus == 0){
break;
}
if(time(NULL) > errorWait + 3){
errlogPrintf("Failed to clear error status after 3 seconds\n");
updateMsgTxtFromDriver("Failed to clear errorState in 3 seconds");
return asynError;
}
}
// now drive to the real place
if(realPosition == 0) {
sprintf(command, "P350=3");
} else if(realPosition < -90){
sprintf(command, "P350=2");
} else {
updateMsgTxtFromDriver("Can only reach -100, 0, nothing else");
return asynError;
}
errlogPrintf("Park position %f, park command: %s\n", realPosition, command);
}
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::move failed to send command %s", command);
updateMsgTxtFromDriver("Cannot send Amor Detector Command");
return status;
}
/* send move command */
if(_function != ADPARK) {
sprintf(command, "P350=1");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::move failed to send command %s", command);
updateMsgTxtFromDriver("Cannot send Amor Detector Command");
}
}
det_starting = true;
det_startTime = time(NULL);
next_poll = BUSYPOLL;
return status;
}
/*-----------------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::stop(double acceleration) {
asynStatus status = asynError;
static const char *functionName = "AmorDetectorAxis::stopAxis";
bool moving = false;
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
this->poll(&moving);
det_starting = false;
if(moving) {
// only send a stop when actually moving
sprintf(command, "P350=8");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
}
return status;
}
/*-------------------------------------------------------------------------------*/
asynStatus AmorDetectorAxis::poll(bool *moving)
{
asynStatus status = asynError;
static const char *functionName = "AmorDetectorAxis::poll";
int targetReached, errorCode, parkCode;
double position;
pC_->debugFlow(functionName);
char command[128] = {0};
char response[32] = {0};
char errorMessage[128] = {0};
/* read in position flag*/
sprintf(command, "P354");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess){
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::poll failed to read in position");
updateMsgTxtFromDriver("Failed to read Amor Detector in position");
return status;
}
sscanf(response, "%d", &targetReached);
//if(_function == ADCOM){
// errlogPrintf("startTime: %ld, current time: %ld, starting flag %d, targetReached %d\n", det_startTime, time(NULL),
// det_starting, targetReached);
//}
// The thing is sometimes slow to start, allow 7 seconds
// Especially coz is really slow to indicate
if(det_starting && time(NULL) > (det_startTime + 7)) {
det_starting = false;
//if(_function == ADCOM){
// errlogPrintf("Resetting starting flag after timeout\n");
//}
}
setIntegerParam(pC_->motorStatusProblem_, false);
if(targetReached == 0 && det_starting == false){
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = IDLEPOLL;
} else {
*moving = true;
if(targetReached == 1){
det_starting = false;
//if(_function == ADCOM){
// errlogPrintf("Resetting starting flag following targetReached flag\n");
//}
}
setIntegerParam(pC_->motorStatusDone_, false);
}
// read position
if(_function == ADCOM) {
strcpy(command,"Q0510");
} else if(_function == ADCOZ) {
strcpy(command, "Q454");
} else if(_function == ADPARK){
strcpy(command,"P358");
}
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status != asynSuccess){
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::poll failed to read value");
updateMsgTxtFromDriver("Failed to read Amor Detector value");
return status;
}
if(_function != ADPARK){
sscanf(response, "%lf", &position);
} else {
// analyse the park code to check where the thing is. This does
// not show good values while driving. But that is a limitation
// of the hardware.
sscanf(response, "%d", &parkCode);
if(parkCode == 1) {
position = 0;
} else {
position = -100;
}
// errlogPrintf("parkCode %d , position %f,targetREached = %d\n", parkCode, position, targetReached);
}
setDoubleParam(pC_->motorPosition_, position * 1000.);
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
//if(_function == ADCOM){
// errlogPrintf("polling for function %d, position %f, pos*10^3 %f, targetREached = %d\n", _function, position,
// position*MULT, targetReached);
// }
// read error status
sprintf(command, "P359");
status = pC_->lowLevelWriteRead(axisNo_, command, response);
if(status == asynSuccess) {
sscanf(response, "%d", &errorCode);
switch(errorCode) {
case 0:
break;
case 1:
strcpy(errorMessage,"COZ not released");
break;
case 2:
strcpy(errorMessage,"Motor is in wrong state");
break;
case 3:
strcpy(errorMessage,"FTZ error while moving");
break;
case 4:
strcpy(errorMessage, "Break while moving");
break;
case 5:
strcpy(errorMessage, "No freigabe during move");
break;
default:
strcpy(errorMessage, "Unknown error code from: ");
strcat(errorMessage, response);
break;
}
if(strlen(errorMessage) > 1) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"AmorDetectorAxis::poll: %s", errorMessage);
updateMsgTxtFromDriver(errorMessage);
setIntegerParam(pC_->motorStatusProblem_, true);
status = asynError;
}
}
callParamCallbacks();
return status;
}

31
sinqEPICSApp/src/pmacAxis.h
View File

@ -157,4 +157,35 @@ public:
friend class pmacV3Controller;
};
/*
* Special motors for the AMOR detector movement. The whole
* command set is different but on a pmac controller. This implements
* a coordinated movement of cox, coz and ftz in order not to break
* the flight tube which may have been mounted. This is mapped to three
* motors selected via the function code: com, the detector omega, coz,
* the detector z offset and park, for parking the flightpath.
*/
#define ADCOM 1
#define ADCOZ 2
#define ADPARK 3
class AmorDetectorAxis: public pmacAxis {
public:
AmorDetectorAxis(pmacController *pController, int axisNo, int function);
asynStatus move(double position, int relative, double min_velocity,
double max_velocity, double acceleration);
asynStatus poll(bool *moving);
asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
asynStatus stop(double acceleration);
asynStatus setPosition(double position);
protected:
int _function;
int det_starting;
time_t det_startTime;
};
#endif /* pmacAxis_H */

40
sinqEPICSApp/src/pmacController.cpp
View File

@ -751,7 +751,33 @@ asynStatus pmacCreateAxes(const char *pmacName,
return asynSuccess;
}
/**
* C wrapper for the pmacAmorDetectorAxis constructor.
* See pmacAmorDetectorAxis::pmacAmorDetectorAxis.
*
*/
asynStatus pmacCreateAmorDetectorAxis(const char *pmacName, /* specify which controller by port name */
int axis, /* axis number (start from 1). */
int function_code) /* AMOR detector axis function */
{
pmacController *pC;
pmacAxis *pAxis;
static const char *functionName = "pmacCreateHRPTAxis";
pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new AmorDetectorAxis(pC, axis, function_code);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/*================================ SeleneController ===============================================*/
@ -996,6 +1022,19 @@ static void configpmacAxesCallFunc(const iocshArgBuf *args)
pmacCreateAxes(args[0].sval, args[1].ival);
}
/* pmacCraeteAmorDetectorAxis */
static const iocshArg pmacCreateAmorDetectorAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAmorDetectorAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg pmacCreateAmorDetectorAxisArg2 = {"Function code", iocshArgInt};
static const iocshArg * const pmacCreateAmorDetectorAxisArgs[] = {&pmacCreateAmorDetectorAxisArg0,
&pmacCreateAmorDetectorAxisArg1, &pmacCreateAmorDetectorAxisArg2};
static const iocshFuncDef configpmacAmorDetectorAxis = {"pmacCreateAmorDetectorAxis", 3, pmacCreateAmorDetectorAxisArgs};
static void configpmacAmorDetectorAxisCallFunc(const iocshArgBuf *args)
{
pmacCreateAmorDetectorAxis(args[0].sval, args[1].ival, args[2].ival);
}
static void pmacControllerRegister(void)
@ -1005,6 +1044,7 @@ static void pmacControllerRegister(void)
iocshRegister(&configpmacV3CreateController, configpmacV3CreateControllerCallFunc);
iocshRegister(&configpmacAxis, configpmacAxisCallFunc);
iocshRegister(&configpmacHRPTAxis, configpmacHRPTAxisCallFunc);
iocshRegister(&configpmacAmorDetectorAxis, configpmacAmorDetectorAxisCallFunc);
iocshRegister(&configSeleneCreateAxis, configSeleneCreateAxisCallFunc);
iocshRegister(&configLiftAxis, configLiftAxisCallFunc);
iocshRegister(&configpmacV3Axis, configpmacV3AxisCallFunc);

1
sinqEPICSApp/src/pmacController.h
View File

@ -140,6 +140,7 @@ class pmacController : public SINQController {
friend class SeleneAxis;
friend class LiftAxis;
friend class pmacV3Axis;
friend class AmorDetectorAxis;
};
#define NUM_PMAC_PARAMS (&LAST_PMAC_PARAM - &FIRST_PMAC_PARAM + 1)