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

Final version of the SINQ EPICS module for RHEL7 in 2023

Browse Source 
- Fixes to MasterMACS, mostly.
- Minor changes
This commit is contained in:
koennecke
2024-01-11 14:05:49 +01:00
parent 9422353107
commit b4e201ae86
9 changed files with 212 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

255
sinqEPICSApp/src/MasterMACSDriver.cpp
View File

@@ -269,15 +269,18 @@ asynStatus
* overwritten when we read back the status at the end, but that's OK */
pAxis->setIntegerParam(function, value);
if(function == motorStop_){
errlogPrintf("Stop called with value %d\n", value);
double accel;
getDoubleParam(pAxis->axisNo_, motorAccel_, &accel);
status = pAxis->stop(accel);
return status;
}
if (function == enableAxis_) {
/*
* Read the status in order to prevent execssive commands
*/
devStatus = pAxis->readStatus();
if(devStatus < 900){
errlogPrintf("MMACS: Motor %d not ready to switch on\n", pAxis->axisNo_);
return asynError;
}
if (value == 1 && !pAxis->isOn(devStatus) ) {
/* download parameters, does not work as of now */
/*
@@ -309,6 +312,7 @@ asynStatus
if(pAxis->isOn(devStatus) == value){
pAxis->active = true;
pAxis->poll(&moving); // to update the Enable_RBV field
pAxis->active = false;
return asynSuccess;
}
usleep(200);
@@ -388,12 +392,11 @@ asynStatus
MasterMACSAxis::move(double position, int relative, double minVelocity,
double maxVelocity, double acceleration)
{
asynStatus status;
asynStatus status = asynSuccess;
char command[COMLEN], reply[COMLEN];
int devStatus;
errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity,
maxVelocity);
//errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity);
memset(command, 0, COMLEN * sizeof(char));
/* clear motor error message */
@@ -409,18 +412,9 @@ asynStatus
}
/*
* test if the thing is On
* only start if the thing is On
*/
devStatus = readStatus();
if(devStatus < 900) {
return asynError;
}
if (!isOn(devStatus)) {
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
errlogPrintf("ERROR: trying to start disabled axis %d\n", axisNo_);
return asynError;
}
/*
* set speed
@@ -431,30 +425,38 @@ asynStatus
*/
if (relative) {
position += this->position;
}
errlogPrintf("Starting axis %d with destination %f", axisNo_,
position / 1000.);
/* send target position */
sprintf(command, "%dS02= %.3f", axisNo_, position / 1000.);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
return status;
position += this->position;
}
/* send move command */
sprintf(command, "%dS00=1", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
return status;
}
hasStarted = true;
homing = 0;
active = true;
if(isOn(devStatus) && active == false) {
errlogPrintf("Starting axis %d with destination %f\n", axisNo_,
position / 1000.);
/* send target position */
sprintf(command, "%dS02= %.3f", axisNo_, position / 1000.);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
errlogPrintf("MMACS: failed to set target on %d\n", axisNo_);
updateMsgTxtFromDriver("Failed to send target position");
return status;
}
/* send move command */
sprintf(command, "%dS00=1", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
errlogPrintf("MMACS: failed to start axis %d\n", axisNo_);
updateMsgTxtFromDriver("Failed to start axis");
return status;
}
hasStarted = true;
homing = 0;
active = true;
usleep(500);
lastPositionUpdate = time(NULL);
} else {
errlogPrintf("MMACS: axis %d disabled, cannot start\n", axisNo_);
}
return status;
}
@@ -472,26 +474,20 @@ asynStatus
* test if the thing is On
*/
devStatus = readStatus();
if(devStatus < 900) {
return asynError;
}
if (!isOn(devStatus)) {
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
errlogPrintf("ERROR: trying to home disabled axis %d\n", axisNo_);
return asynError;
}
setIntegerParam(pC_->motorStatusProblem_, false);
updateMsgTxtFromDriver("");
sprintf(command, "%dS00=9", axisNo_);
homing = 1;
status = pC_->transactController(axisNo_, command, reply);
hasStarted = true;
active = true;
return status;
if(isOn(devStatus)){
sprintf(command, "%dS00=9", axisNo_);
homing = 1;
status = pC_->transactController(axisNo_, command, reply);
hasStarted = true;
active = true;
lastPositionUpdate = time(NULL);
return status;
} else {
errlogPrintf("MMACS: cannot home disabled axis %d\n", axisNo_);
return asynError;
}
}
asynStatus
@@ -552,9 +548,8 @@ asynStatus MasterMACSAxis::poll(bool * moving)
{
asynStatus comStatus = asynSuccess;
char command[COMLEN], reply[COMLEN], *pPtr, buffer[80];
float errStatus;
unsigned int errCode, derCode, devStatus;
float errStatus;
struct tm* tm_info;
time_t timer;
@@ -574,6 +569,7 @@ asynStatus MasterMACSAxis::poll(bool * moving)
strftime(buffer, 26, "%Y-%m-%d %H:%M:%S", tm_info);
errlogPrintf("poll called at %s on axis %d \n",
buffer, axisNo_ );
lastPoll = time(NULL);
setIntegerParam(pC_->motorStatusProblem_, false);
memset(command, 0, COMLEN * sizeof(char));
@@ -581,48 +577,51 @@ asynStatus MasterMACSAxis::poll(bool * moving)
// Read the current motor position
sprintf(command, "%dR12", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
pPtr = strstr(reply, "=");
if (pPtr) {
sscanf(pPtr + 1, "%lf", &position);
if(comStatus != asynError) {
pPtr = strstr(reply, "=");
if(pPtr) {
sscanf(pPtr + 1, "%lf", &position);
setDoubleParam(pC_->motorPosition_, position * 1000.);
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
/*
* keep track of position in order to check for a stuck motor later
*/
if(ABS(position - oldPosition) > 1){
oldPosition = position;
lastPositionUpdate = time(NULL);
}
} else {
errlogPrintf("MMACS: Invalid response asking position on %d\n", axisNo_);
}
} else {
errlogPrintf("Received malformed reply: Axis %d, reply %s\n",
axisNo_, reply + 4);
comStatus = asynError;
goto skip;
}
setDoubleParam(pC_->motorPosition_, position * 1000.);
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
/*
* keep track of position in order to check for a stuck motor later
*/
if(ABS(position - oldPosition) > 1){
oldPosition = position;
lastPositionUpdate = time(NULL);
errlogPrintf("MMACS: communication problem reading axis %d position\n", axisNo_);
}
// Read the overall status of this motor */
devStatus = readStatus();
if(devStatus < 900) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
if(debug) {
errlogPrintf("Axis %d, position %lf, devStatus %d\n", axisNo_,
position, devStatus);
}
// Check for the thing being in a bad state
if(CHECK_BIT(devStatus, 6)) {
*moving = false;
active = false;
setIntegerParam(pC_->motorStatusDone_, true);
updateMsgTxtFromDriver("AXIS dead");
goto skip;
}
setIntegerParam(pC_->enableAxis_, isOn(devStatus));
setIntegerParam(pC_->axisEnabled_, isOn(devStatus));
// Check if the motor is disabled
if (!isOn(devStatus)) {
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
updateMsgTxtFromDriver("Axis disabled");
*moving = false;
active = false;
setIntegerParam(pC_->motorStatusDone_, true);
goto skip;
}
@@ -631,9 +630,10 @@ asynStatus MasterMACSAxis::poll(bool * moving)
* if the motor has never run, the status bit 10 is invalid
*/
if (!hasStarted) {
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
goto skip;
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
active = false;
goto skip;
}
/*
@@ -643,11 +643,13 @@ asynStatus MasterMACSAxis::poll(bool * moving)
/* we are still creeping along .... */
*moving = true;
setIntegerParam(pC_->motorStatusDone_, false);
if(time(NULL) > lastPositionUpdate + 60) {
if(time(NULL) > lastPositionUpdate + 120) {
// we are detecting a stuck motor
errlogPrintf("MMACS: axis %d is STUCK!!\n", axisNo_);
updateMsgTxtFromDriver("Axis STUCK!!");
setIntegerParam(pC_->motorStatusProblem_, true);
setIntegerParam(pC_->motorStatusDone_, true);
*moving = false;
active = false;
}
goto skip;
}
@@ -657,29 +659,25 @@ asynStatus MasterMACSAxis::poll(bool * moving)
*moving = false;
active = false;
setIntegerParam(pC_->motorStatusDone_, true);
updateMsgTxtFromDriver("");
/* when homing, set the proper flag */
if (homing) {
setIntegerParam(pC_->motorStatusAtHome_, true);
setIntegerParam(pC_->motorStatusAtHome_, true);
}
/* check for limit switches*/
setIntegerParam(pC_->motorStatusLowLimit_, false);
setIntegerParam(pC_->motorStatusHighLimit_, false);
if (CHECK_BIT(devStatus, 11)) {
setIntegerParam(pC_->motorStatusProblem_, true);
errlogSevPrintf(errlogMajor,
"Limit bit in status code %d",
axisNo_);
errlogSevPrintf(errlogMajor, "Limit bit in status code %d", axisNo_);
/* guessing which limit has been hit ... */
if (position > 0) {
updateMsgTxtFromDriver("Hit positive limit switch");
setIntegerParam(pC_->motorStatusHighLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
updateMsgTxtFromDriver("Hit positive limit switch");
setIntegerParam(pC_->motorStatusHighLimit_, true);
} else {
updateMsgTxtFromDriver("Hit negative limit switch");
setIntegerParam(pC_->motorStatusLowLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
updateMsgTxtFromDriver("Hit negative limit switch");
setIntegerParam(pC_->motorStatusLowLimit_, true);
}
goto skip;
}
@@ -689,44 +687,45 @@ asynStatus MasterMACSAxis::poll(bool * moving)
/* read error codes */
sprintf(command, "%dR11", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
pPtr = strstr(reply, "=");
if(pPtr) {
sscanf(pPtr + 1, "%f", &errStatus);
errCode = (unsigned int) errStatus;
if (comStatus != asynError) {
pPtr = strstr(reply, "=");
if(pPtr) {
sscanf(pPtr + 1, "%f", &errStatus);
errCode = (unsigned int) errStatus;
} else {
errlogPrintf("MMACS: axis %d received invalid reply asking for error code \n", axisNo_);
errCode = 0;
goto skip;
}
} else {
errlogPrintf("MMACS: axis %d received invalid reply asking for error code \n", axisNo_);
updateMsgTxtFromDriver("Invalid reply asking error code R11");
errCode = 0;
errlogPrintf("MMACS: axis %d failed reading error code \n", axisNo_);
goto skip;
}
sprintf(command, "%dR18", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
pPtr = strstr(reply, "=");
if(pPtr) {
sscanf(pPtr + 1, "%f", &errStatus);
derCode = (unsigned int) errStatus;
if (comStatus != asynError) {
pPtr = strstr(reply, "=");
if(pPtr) {
sscanf(pPtr + 1, "%f", &errStatus);
derCode = (unsigned int) errStatus;
} else {
errlogPrintf("MMACS: malformed reply for R18: %s, on axis %d\n", reply, axisNo_);
derCode = 0;
goto skip;
}
} else {
errlogPrintf("MMACS: malformed reply for R18: %s, on axis %d\n", reply, axisNo_);
derCode = 0;
updateMsgTxtFromDriver("Invalid reply asking error code R18");
errlogPrintf("MMACS: axis %d failed reading extended error code R18 \n", axisNo_);
goto skip;
}
if(debug) {
errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
errCode, derCode);
}
}
setIntegerParam(pC_->motorStatusProblem_, true);
if (errCode == 0) {
setIntegerParam(pC_->motorStatusProblem_, true);
errlogSevPrintf(errlogMajor,
"Fault bit in status code, but no error code on %d\n", axisNo_);
updateMsgTxtFromDriver ("Fault bit in status code without error code");