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

Many improvements to the MasterMACS motor driver. It is working but the hardware is shaky.

Browse Source 
Added support for dose rate controlled Phytron motors. Not tested!
Small bug fixes
This commit is contained in:
koennecke
2023-05-31 09:13:41 +02:00
parent f333a27482
commit 72afd02b4e
9 changed files with 394 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

357
sinqEPICSApp/src/MasterMACSDriver.cpp
View File

@@ -141,6 +141,12 @@ asynStatus
SINQAxis *axis = getAxis(axisNo);
pasynOctetSyncIO->flush(pasynUserController_);
/* read with a short timeout in order to remove duplicate messages
* from the line. This also serves to slow down communication
*/
pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .4, &in, &reason);
/* pack data for MasterMACS */
len = strlen(command) + 6;
mmacsData = (char *) malloc(len * sizeof(char));
@@ -158,7 +164,7 @@ asynStatus
/* 0x03 is appended by asyn */
/* send the stuff away ... */
//errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
status =
pasynOctetSyncIO->writeRead(pasynUserController_, mmacsData,
@@ -180,13 +186,14 @@ asynStatus
}
free(mmacsData);
/* format and print the response in hex for debugging purposes
/* format and print the response in hex for debugging purposes */
/*
for(i = 0, idx = 0; i < in; i++){
sprintf(hexResponse + idx, "%02x ", (unsigned int)mmacsResponse[i]);
idx = strlen(hexResponse);
}
errlogSevPrintf(errlogMajor,"Reply in hex: %s\n", hexResponse);
*/
*/
/* Here we have read the data from the MasterMACS. We proceed to extract
* the payload and the state of the ACK byte and place that reply into data
@@ -227,7 +234,7 @@ asynStatus
lenPayload = idx - 4;
memcpy(reply + 4, mmacsResponse + 4, lenPayload);
reply[4 + lenPayload] = (char) 0;
//errlogSevPrintf(errlogMajor, "Completed reply at end of transactController: %s ", reply);
errlogSevPrintf(errlogMajor, "Completed reply at end of transactController: %s ", reply);
return status;
}
@@ -241,6 +248,7 @@ asynStatus
MasterMACSAxis *pAxis = NULL;
char command[64] = { 0 };
char response[64] = { 0 };
int devStatus;
pAxis = (MasterMACSAxis *) this->getAxis(pasynUser);
if (!pAxis) {
@@ -252,19 +260,29 @@ asynStatus
pAxis->setIntegerParam(function, value);
if (function == enableAxis_) {
if (value == 1) {
/* download parameters, does not work as of now */
/*
sprintf(command, "%dS85=1.", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
*/
/*
* Read the status in order to prevent execssiove commands
*/
devStatus = pAxis->readStatus();
if(devStatus < 900){
return asynError;
}
if (value == 1 && !pAxis->isOn(devStatus) ) {
/* download parameters, does not work as of now */
/*
sprintf(command, "%dS85=1.", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
*/
/* actually enable */
sprintf(command, "%dS04=1.0", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
} else {
sprintf(command, "%dS04=0.0", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
/* actually enable */
sprintf(command, "%dS04=1", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
} else {
if(pAxis->isOn(devStatus)) {
// only send command when necessary
sprintf(command, "%dS04=0", pAxis->axisNo_);
status = transactController(pAxis->axisNo_, command, response);
}
}
if (status == asynSuccess) {
pAxis->updateMsgTxtFromDriver("");
@@ -300,8 +318,13 @@ asynStatus
return asynError;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
if(pPtr) {
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
} else {
devStatus = 0;
errlogPrintf(" Bad reply %s when trying to read motor status %d\n", reply, pAxis->axisNo_);
}
isOn = pAxis->isOn(devStatus);
/* errlogPrintf("isOn in readInt32: %d, devStatus = %d\n", isOn, devStatus); */
setIntegerParam(axisEnabled_, isOn);
@@ -343,6 +366,24 @@ void MasterMACSAxis::report(FILE * fp, int level)
}
}
int MasterMACSAxis::readStatus()
{
char command[COMLEN], reply[COMLEN], *pPtr;
float fval;
int devStatus, status;
sprintf(command, "%dR10", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
return -1000;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
return devStatus;
}
int MasterMACSAxis::isOn(int axisStatus)
{
if (CHECK_BIT(axisStatus, 0) && CHECK_BIT(axisStatus, 1)) {
@@ -356,9 +397,8 @@ asynStatus
double maxVelocity, double acceleration)
{
asynStatus status;
char command[COMLEN], reply[COMLEN], *pPtr;
char command[COMLEN], reply[COMLEN];
int devStatus;
float fval;
errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity,
maxVelocity);
@@ -370,33 +410,33 @@ asynStatus
/*
* reset error code
*/
sprintf(command, "%dS17=0", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if(errorCodeFound){
sprintf(command, "%dS17=0", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
errorCodeFound = 0;
}
/*
* test if the thing is On
*/
sprintf(command, "%dR10", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
return asynError;
devStatus = readStatus();
if(devStatus < 900) {
return asynError;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
if (!isOn(devStatus)) {
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
errlogPrintf("ERROR: trying to start disabled axis %d\n", axisNo_);
return asynError;
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
errlogPrintf("ERROR: trying to start disabled axis %d\n", axisNo_);
return asynError;
}
/*
* set speed
*/
/* temporarily disabled in order to get the basic logic right
sprintf(command, "%dS05=%f", axisNo_, maxVelocity / 1000);
status = pC_->transactController(axisNo_, command, reply);
*/
if (relative) {
position += this->position;
@@ -432,28 +472,23 @@ asynStatus
double acceleration, int forwards)
{
asynStatus status;
char command[COMLEN], reply[COMLEN], *pPtr;
char command[COMLEN], reply[COMLEN];
int devStatus;
float fval;
memset(command, 0, COMLEN * sizeof(char));
/*
* test if the thing is On
*/
sprintf(command, "%dR10", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
return asynError;
devStatus = readStatus();
if(devStatus < 900) {
return asynError;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
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_, true);
updateMsgTxtFromDriver("Motor disabled");
errlogPrintf("ERROR: trying to home disabled axis %d\n", axisNo_);
return asynError;
}
setIntegerParam(pC_->motorStatusProblem_, false);
@@ -483,15 +518,20 @@ asynStatus MasterMACSAxis::stop(double acceleration)
{
asynStatus status = asynSuccess;
char command[COMLEN], reply[COMLEN];
int devStatus;
memset(command, 0, COMLEN * sizeof(char));
if (errorReported == 0) {
sprintf(command, "%dS00=8", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
updateMsgTxtFromDriver("Axis interrupted");
errorReported = 1;
devStatus = readStatus();
if(!CHECK_BIT(devStatus, 10)) {
// only try to stop when running ...
sprintf(command, "%dS00=8", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
updateMsgTxtFromDriver("Axis interrupted");
errorReported = 1;
}
}
return status;
}
@@ -525,7 +565,7 @@ asynStatus MasterMACSAxis::poll(bool * moving)
{
asynStatus comStatus = asynSuccess;
char command[COMLEN], reply[COMLEN], *pPtr;
float errStatus, fval;
float errStatus;
unsigned int errCode, derCode, devStatus;
// protect against excessive polling
@@ -560,25 +600,21 @@ asynStatus MasterMACSAxis::poll(bool * moving)
// Read the overall status of this motor */
sprintf(command, "%dR10", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
devStatus = readStatus();
if(devStatus < 900) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
errlogPrintf("Axis %d, position %lf, devStatus %d\n", axisNo_,
position, devStatus);
if (!isOn(devStatus)) {
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
goto skip;
setIntegerParam(pC_->motorStatusProblem_, true);
updateMsgTxtFromDriver("Motor disabled");
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
goto skip;
}
/*
@@ -613,48 +649,13 @@ asynStatus MasterMACSAxis::poll(bool * moving)
setIntegerParam(pC_->motorStatusAtHome_, true);
}
/* 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, "=");
sscanf(pPtr + 1, "%f", &errStatus);
errCode = (unsigned int) errStatus;
sprintf(command, "%dR18", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &errStatus);
derCode = (unsigned int) errStatus;
errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
errCode, derCode);
setIntegerParam(pC_->motorStatusLowLimit_, false);
/* check for limit switches*/
setIntegerParam(pC_->motorStatusLowLimit_, false);
setIntegerParam(pC_->motorStatusHighLimit_, false);
if (errCode == 0) {
/* There still may be errors in the status code */
if (CHECK_BIT(devStatus, 3)) {
if (CHECK_BIT(devStatus, 11)) {
setIntegerParam(pC_->motorStatusProblem_, true);
errlogSevPrintf(errlogMajor,
"Fault bit in status code, but no error code on %d",
axisNo_);
updateMsgTxtFromDriver
("Fault bit in status code without error code");
}
if (CHECK_BIT(devStatus, 11)) {
setIntegerParam(pC_->motorStatusProblem_, true);
errlogSevPrintf(errlogMajor,
"Limit bit in status code, but no error code on %d",
"Limit bit in status code %d",
axisNo_);
/* guessing which limit has been hit ... */
if (position > 0) {
@@ -666,69 +667,99 @@ asynStatus MasterMACSAxis::poll(bool * moving)
setIntegerParam(pC_->motorStatusLowLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
}
goto skip;
}
goto skip;
}
/* check for error conditions */
if (CHECK_BIT(devStatus, 3)) {
/* 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, "=");
sscanf(pPtr + 1, "%f", &errStatus);
errCode = (unsigned int) errStatus;
/*
* from here on we are processing errors
*/
sprintf(command, "%dR18", axisNo_);
comStatus = pC_->transactController(axisNo_, command, reply);
if (comStatus == asynError) {
setIntegerParam(pC_->motorStatusProblem_, true);
goto skip;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &errStatus);
derCode = (unsigned int) errStatus;
setIntegerParam(pC_->motorStatusProblem_, true);
errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
errCode, derCode);
if (CHECK_BIT(errCode, 0)) {
errlogSevPrintf(errlogMajor, "CAN error on %d", axisNo_);
updateMsgTxtFromDriver("CAN error");
} else if (CHECK_BIT(errCode, 1)) {
errlogSevPrintf(errlogMajor, "Short circuit on %d", axisNo_);
updateMsgTxtFromDriver("Short circuit");
} else if (CHECK_BIT(errCode, 2)) {
errlogSevPrintf(errlogMajor, "Invalide Setup on %d", axisNo_);
updateMsgTxtFromDriver("Invalid Setup");
} else if (CHECK_BIT(errCode, 3)) {
errlogSevPrintf(errlogMajor, "Control error on %d", axisNo_);
updateMsgTxtFromDriver("Control error");
} else if (CHECK_BIT(errCode, 4)) {
errlogSevPrintf(errlogMajor, "CAN communication error on %d",
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");
goto skip;
}
errorCodeFound = 1;
if (CHECK_BIT(errCode, 0)) {
errlogSevPrintf(errlogMajor, "CAN error on %d", axisNo_);
updateMsgTxtFromDriver("CAN error");
} else if (CHECK_BIT(errCode, 1)) {
errlogSevPrintf(errlogMajor, "Short circuit on %d", axisNo_);
updateMsgTxtFromDriver("Short circuit");
} else if (CHECK_BIT(errCode, 2)) {
errlogSevPrintf(errlogMajor, "Invalide Setup on %d", axisNo_);
updateMsgTxtFromDriver("Invalid Setup");
} else if (CHECK_BIT(errCode, 3)) {
errlogSevPrintf(errlogMajor, "Control error on %d", axisNo_);
updateMsgTxtFromDriver("Control error");
} else if (CHECK_BIT(errCode, 4)) {
errlogSevPrintf(errlogMajor, "CAN communication error on %d",
axisNo_);
updateMsgTxtFromDriver("CAN communication error");
} else if (CHECK_BIT(errCode, 5)) {
errlogSevPrintf(errlogMajor, "Feedback error on %d", axisNo_);
updateMsgTxtFromDriver("Feedback error");
} else if (CHECK_BIT(errCode, 6)) {
updateMsgTxtFromDriver("Hit positive limit switch");
setIntegerParam(pC_->motorStatusHighLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
} else if (CHECK_BIT(errCode, 7)) {
updateMsgTxtFromDriver("Hit negative limit switch");
setIntegerParam(pC_->motorStatusLowLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
} else if (CHECK_BIT(errCode, 8)) {
errlogSevPrintf(errlogMajor, "Over current %d", axisNo_);
updateMsgTxtFromDriver("Over current");
} else if (CHECK_BIT(errCode, 9)) {
errlogSevPrintf(errlogMajor, "I2T protection on %d", axisNo_);
updateMsgTxtFromDriver("I2t protection");
} else if (CHECK_BIT(errCode, 10)) {
errlogSevPrintf(errlogMajor, "Over heated motor on %d", axisNo_);
updateMsgTxtFromDriver("Motor overheated");
} else if (CHECK_BIT(errCode, 11)) {
errlogSevPrintf(errlogMajor, "Over temperature drive on %d",
axisNo_);
updateMsgTxtFromDriver("CAN communication error");
} else if (CHECK_BIT(errCode, 5)) {
errlogSevPrintf(errlogMajor, "Feedback error on %d", axisNo_);
updateMsgTxtFromDriver("Feedback error");
} else if (CHECK_BIT(errCode, 6)) {
updateMsgTxtFromDriver("Hit positive limit switch");
setIntegerParam(pC_->motorStatusHighLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
} else if (CHECK_BIT(errCode, 7)) {
updateMsgTxtFromDriver("Hit negative limit switch");
setIntegerParam(pC_->motorStatusLowLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
} else if (CHECK_BIT(errCode, 8)) {
errlogSevPrintf(errlogMajor, "Over current %d", axisNo_);
updateMsgTxtFromDriver("Over current");
} else if (CHECK_BIT(errCode, 9)) {
errlogSevPrintf(errlogMajor, "I2T protection on %d", axisNo_);
updateMsgTxtFromDriver("I2t protection");
} else if (CHECK_BIT(errCode, 10)) {
errlogSevPrintf(errlogMajor, "Over heated motor on %d", axisNo_);
updateMsgTxtFromDriver("Motor overheated");
} else if (CHECK_BIT(errCode, 11)) {
errlogSevPrintf(errlogMajor, "Over temperature drive on %d",
axisNo_);
updateMsgTxtFromDriver("Over temperature drive");
} else if (CHECK_BIT(errCode, 12)) {
errlogSevPrintf(errlogMajor, "Over voltage on %d", axisNo_);
updateMsgTxtFromDriver("Over voltage");
} else if (CHECK_BIT(errCode, 13)) {
errlogSevPrintf(errlogMajor, "Under voltage on %d", axisNo_);
updateMsgTxtFromDriver("Under voltage");
} else if (CHECK_BIT(errCode, 14)) {
errlogSevPrintf(errlogMajor, "Command error on %d", axisNo_);
updateMsgTxtFromDriver("Command error");
} else if (CHECK_BIT(errCode, 15)) {
errlogSevPrintf(errlogMajor, "Motor disabled on %d", axisNo_);
updateMsgTxtFromDriver("Motor disabled");
}
updateMsgTxtFromDriver("Over temperature drive");
} else if (CHECK_BIT(errCode, 12)) {
errlogSevPrintf(errlogMajor, "Over voltage on %d", axisNo_);
updateMsgTxtFromDriver("Over voltage");
} else if (CHECK_BIT(errCode, 13)) {
errlogSevPrintf(errlogMajor, "Under voltage on %d", axisNo_);
updateMsgTxtFromDriver("Under voltage");
} else if (CHECK_BIT(errCode, 14)) {
errlogSevPrintf(errlogMajor, "Command error on %d", axisNo_);
updateMsgTxtFromDriver("Command error");
} else if (CHECK_BIT(errCode, 15)) {
errlogSevPrintf(errlogMajor, "Motor disabled on %d", axisNo_);
updateMsgTxtFromDriver("Motor disabled");
}
}
skip:
callParamCallbacks();