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

First working version of the MasterMACS EPICS driver

Browse Source 
Also added some test code
This commit is contained in:
koennecke
2023-03-21 14:55:07 +01:00
parent b8896b7a85
commit ccd73babd5
22 changed files with 20883 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

820
sinqEPICSApp/src/MasterMACSDriver.cpp Normal file
View File

@ -0,0 +1,820 @@
/*
Driver for the MasterMACS motor controller used at SINQ
For documentation see the standard SINQ place for hardware documentation or
Marcel Schildt
Mark Koennecke, March 2023
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <time.h>
#include <iocsh.h>
#include <epicsThread.h>
#include <errlog.h>
#include <asynOctetSyncIO.h>
#include "MasterMACSDriver.h"
#include <epicsExport.h>
#define IDLEPOLL 60
#define CHECK_BIT(var,pos) ((var) & (1 << pos))
#define ABS(x) (x < 0 ? -(x) : (x))
/** Creates a new MasterMACSController object.
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] MasterMACSPortName The name of the drvAsynSerialPort that was created previously to connect to the MasterMACS controller
* \param[in] numAxes The number of axes that this controller supports
*/
MasterMACSController::MasterMACSController(const char *portName,
const char *MasterMACSPortName,
int
numAxes):SINQController
(portName, MasterMACSPortName, numAxes)
{
asynStatus status;
static const char *functionName =
"MasterMACSController::MasterMACSController";
char terminator[2] = "\x03";
/* Connect to MasterMACS controller */
status =
pasynOctetSyncIO->connect(MasterMACSPortName, 0,
&pasynUserController_, NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to MasterMACS controller\n",
functionName);
}
pasynOctetSyncIO->setOutputEos(pasynUserController_, terminator,
strlen(terminator));
pasynOctetSyncIO->setInputEos(pasynUserController_, terminator,
strlen(terminator));
pAxes_ = (MasterMACSAxis **) (asynMotorController::pAxes_);
createParam(EnableAxisString, asynParamInt32, &enableAxis_);
createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
callParamCallbacks();
startPoller(1000. / 1000., IDLEPOLL, 2);
}
/** Creates a new MasterMACSController object.
* Configuration command, called directly or from iocsh
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] MasterMACSPortName The name of the drvAsynIPPPort that was created previously to connect to the MasterMACS controller
* \param[in] numAxes The number of axes that this controller supports
*/
extern "C" int
MasterMACSCreateController(const char *portName,
const char *MasterMACSPortName, int numAxes)
{
MasterMACSController *pMasterMACSController
= new MasterMACSController(portName, MasterMACSPortName, numAxes);
pMasterMACSController = NULL;
return (asynSuccess);
}
/** Reports on status of the driver
* \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired
*
* If details > 0 then information is printed about each axis.
* After printing controller-specific information it calls asynMotorController::report()
*/
void
MasterMACSController::report(FILE * fp, int level)
{
fprintf(fp, "MasterMACS motor driver %s, numAxes=%d\n",
this->portName, numAxes_);
// Call the base class method
asynMotorController::report(fp, level);
}
/** Returns a pointer to an MasterMACSAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
MasterMACSAxis *MasterMACSController::getAxis(asynUser * pasynUser)
{
return static_cast <
MasterMACSAxis * >(asynMotorController::getAxis(pasynUser));
}
/** Returns a pointer to an MasterMACSAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] axisNo Axis index number. */
MasterMACSAxis *MasterMACSController::getAxis(int axisNo)
{
return static_cast <
MasterMACSAxis * >(asynMotorController::getAxis(axisNo));
}
/**
* send a command to the MasterMACS and read the reply. Do some error and controller
* issue fixing on the way
* \param[in] command The command to send
* \param[out] reply The controllers reply
*/
asynStatus
MasterMACSController::transactController(int axisNo,
char command[COMLEN],
char reply[COMLEN])
{
asynStatus status;
size_t in, out;
int reason, len, idx, lenPayload;
unsigned int i;
char *mmacsData =
NULL, ackchar, mmacsResponse[COMLEN], hexResponse[256];
SINQAxis *axis = getAxis(axisNo);
pasynOctetSyncIO->flush(pasynUserController_);
/* pack data for MasterMACS */
len = strlen(command) + 6;
mmacsData = (char *) malloc(len * sizeof(char));
if (!mmacsData) {
errlogSevPrintf(errlogMajor,
"Failed to allocate memory in MasterMACSController::transactController");
return asynError;
}
mmacsData[0] = 0x05;
mmacsData[1] = (char) (len - 2);
mmacsData[2] = 0;
mmacsData[3] = 0x19;
memcpy(mmacsData + 4, command, strlen(command) * sizeof(char));
mmacsData[len - 2] = 0x0D;
/* 0x03 is appended by asyn */
/* send the stuff away ... */
//errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
status =
pasynOctetSyncIO->writeRead(pasynUserController_, mmacsData,
len - 1, mmacsResponse, 35, 5., &out,
&in, &reason);
if (status != asynSuccess) {
if (axis != NULL) {
errlogSevPrintf(errlogMajor,
"Lost connection to motor controller, reason %d",
reason);
axis->updateMsgTxtFromDriver
("Lost connection to motor controller");
return status;
}
errlogSevPrintf(errlogMajor,
"Lost connection to motor controller without axis, reason %d",
reason);
return status;
}
free(mmacsData);
/* 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
*/
if ((in < 33)) {
errlogSevPrintf(errlogMajor,
"MasterMACS only sent %d bytes, 34 expected",
(int) in);
return asynError;
}
/*
* Unpack the MasterMACS message, start by finding <CR>
*/
for (i = 4, idx = 0; i < 34; i++) {
if (mmacsResponse[i] == 0x0d) {
idx = i;
break;
}
}
//errlogSevPrintf(errlogMajor, "Found <CR> at %d", idx);
/* one character before <CR> is the ACK, if it is there */
ackchar = mmacsResponse[idx - 1];
if (ackchar == 0x06) {
strcpy(reply, "ACK:");
} else if (ackchar == 0x15) {
strcpy(reply, "NAK:");
errlogSevPrintf(errlogMajor,
"MasterMACS responded with NAK to %s\n", command);
status = asynError;
} else {
/* the MasterMACS does not always send a ACK/NAK on read */
strcpy(reply, "NON:");
}
//errlogSevPrintf(errlogMajor, "Reply after testing ACK byte: %s ", reply);
/* copy data */
lenPayload = idx - 4;
memcpy(reply + 4, mmacsResponse + 4, lenPayload);
reply[4 + lenPayload] = (char) 0;
//errlogSevPrintf(errlogMajor, "Completed reply at end of transactController: %s ", reply);
return status;
}
asynStatus
MasterMACSController::writeInt32(asynUser * pasynUser,
epicsInt32 value)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
MasterMACSAxis *pAxis = NULL;
char command[64] = { 0 };
char response[64] = { 0 };
pAxis = (MasterMACSAxis *) this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* Set the parameter and readback in the parameter library. This may be
* overwritten when we read back the status at the end, but that's OK */
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);
*/
/* 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);
}
if (status == asynSuccess) {
pAxis->updateMsgTxtFromDriver("");
} else {
errlogPrintf("Failure to enable or disable axis %d",
pAxis->axisNo_);
}
}
return asynMotorController::writeInt32(pasynUser, value);
}
asynStatus
MasterMACSController::readInt32(asynUser * pasynUser,
epicsInt32 * value)
{
int function = pasynUser->reason;
MasterMACSAxis *pAxis = NULL;
char command[128] = { 0 }, reply[128] = {
0}, *pPtr;
float fval;
int devStatus, isOn, comStatus;
pAxis = (MasterMACSAxis *) (this->getAxis(pasynUser));
if (!pAxis) {
return asynError;
}
if (function == axisEnabled_) {
// Read the overall status of this motor */
sprintf(command, "%dR10", pAxis->axisNo_);
comStatus = transactController(pAxis->axisNo_, command, reply);
if (comStatus == asynError) {
return asynError;
}
pPtr = strstr(reply, "=");
sscanf(pPtr + 1, "%f", &fval);
devStatus = (int) fval;
isOn = pAxis->isOn(devStatus);
/* errlogPrintf("isOn in readInt32: %d, devStatus = %d\n", isOn, devStatus); */
setIntegerParam(axisEnabled_, isOn);
*value = isOn;
callParamCallbacks();
return asynSuccess;
}
return asynMotorController::readInt32(pasynUser, value);
}
// These are the MasterMACSAxis methods
/** Creates a new MasterMACSAxis object.
* \param[in] pC Pointer to the MasterMACSController to which this axis belongs.
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
*
* Initializes register numbers, etc.
*/
MasterMACSAxis::MasterMACSAxis(MasterMACSController * pC, int axisNo):SINQAxis(pC, axisNo),
pC_
(pC)
{
next_poll = -1;
hasStarted = false;
errorReported = 1;
}
/** Reports on status of the axis
* \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired
*
* After printing device-specific information calls asynMotorAxis::report()
*/
void MasterMACSAxis::report(FILE * fp, int level)
{
if (level > 0) {
fprintf(fp, " axis %d\n", axisNo_);
}
}
int MasterMACSAxis::isOn(int axisStatus)
{
if (CHECK_BIT(axisStatus, 0) && CHECK_BIT(axisStatus, 1)) {
return 1;
}
return 0;
}
asynStatus
MasterMACSAxis::move(double position, int relative, double minVelocity,
double maxVelocity, double acceleration)
{
asynStatus status;
char command[COMLEN], reply[COMLEN], *pPtr;
int devStatus;
float fval;
errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity,
maxVelocity);
memset(command, 0, COMLEN * sizeof(char));
/* clear motor error message */
updateMsgTxtFromDriver("");
/*
* reset error code
*/
sprintf(command, "%dS17=0", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
/*
* test if the thing is On
*/
sprintf(command, "%dR10", axisNo_);
status = pC_->transactController(axisNo_, command, reply);
if (status == asynError) {
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;
}
/*
* set speed
*/
sprintf(command, "%dS05=%f", axisNo_, maxVelocity / 1000);
status = pC_->transactController(axisNo_, command, reply);
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;
}
/* 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;
next_poll = -1;
homing = 0;
errorReported = 0;
return status;
}
asynStatus
MasterMACSAxis::home(double minVelocity, double maxVelocity,
double acceleration, int forwards)
{
asynStatus status;
char command[COMLEN], reply[COMLEN], *pPtr;
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;
}
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_, false);
updateMsgTxtFromDriver("");
errorReported = 0;
sprintf(command, "%dS00=9", axisNo_);
homing = 1;
next_poll = -1;
status = pC_->transactController(axisNo_, command, reply);
hasStarted = true;
return status;
}
asynStatus
MasterMACSAxis::moveVelocity(double minVelocity, double maxVelocity,
double acceleration)
{
setIntegerParam(pC_->motorStatusProblem_, true);
errlogSevPrintf(errlogMajor,
"This controller does not support the jog feature");
return asynError;
}
asynStatus MasterMACSAxis::stop(double acceleration)
{
asynStatus status = asynSuccess;
char command[COMLEN], reply[COMLEN];
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;
}
return status;
}
asynStatus MasterMACSAxis::setPosition(double position)
{
setIntegerParam(pC_->motorStatusProblem_, true);
errlogSevPrintf(errlogMajor,
"This controller does not support setting position");
updateMsgTxtFromDriver("Controller does not support setPosition");
return asynError;
}
asynStatus MasterMACSAxis::setClosedLoop(bool closedLoop)
{
/*
This belongs into the Kingdom of Electronics.
We do not do this.
*/
return asynError;
}
/** Polls the axis.
* This function reads the motor position, the limit status, the home status, the moving status,
* and the drive power-on status.
* It calls setIntegerParam() and setDoubleParam() for each item that it polls,
* and then calls callParamCallbacks() at the end.
* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus MasterMACSAxis::poll(bool * moving)
{
asynStatus comStatus = asynSuccess;
char command[COMLEN], reply[COMLEN], *pPtr;
float errStatus, fval;
unsigned int errCode, derCode, devStatus;
// protect against excessive polling
if (time(NULL) < next_poll) {
*moving = false;
return asynSuccess;
}
setIntegerParam(pC_->motorStatusProblem_, false);
memset(command, 0, COMLEN * sizeof(char));
// 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);
} else {
errlogPrintf("Received malformed reply: Axis %d, reply %s\n",
axisNo_, reply + 4);
return asynError;
}
setDoubleParam(pC_->motorPosition_, position * 1000.);
setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
// 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;
}
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;
}
/*
* if the motor has never run, the status bit 10 is invalid
*/
if (!hasStarted) {
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = time(NULL) + IDLEPOLL;
goto skip;
}
/*
* We may have a valid status bit...
*/
if (!CHECK_BIT(devStatus, 10)) {
/* we are still creeping along .... */
*moving = true;
next_poll = -1;
setIntegerParam(pC_->motorStatusDone_, false);
goto skip;
}
/*we are done moving */
*moving = false;
setIntegerParam(pC_->motorStatusDone_, true);
next_poll = time(NULL) + IDLEPOLL;
/* when homing, set the proper flag */
if (homing) {
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);
setIntegerParam(pC_->motorStatusHighLimit_, false);
if (errCode == 0) {
/* There still may be errors in the status code */
if (CHECK_BIT(devStatus, 3)) {
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",
axisNo_);
/* guessing which limit has been hit ... */
if (position > 0) {
updateMsgTxtFromDriver("Hit positive limit switch");
setIntegerParam(pC_->motorStatusHighLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
} else {
updateMsgTxtFromDriver("Hit negative limit switch");
setIntegerParam(pC_->motorStatusLowLimit_, true);
setIntegerParam(pC_->motorStatusProblem_, false);
}
}
goto skip;
}
/*
* from here on we are processing errors
*/
setIntegerParam(pC_->motorStatusProblem_, true);
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("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();
return comStatus;
}
/** Code for iocsh registration */
static const iocshArg
MasterMACSCreateControllerArg0 = { "Port name", iocshArgString };
static const iocshArg
MasterMACSCreateControllerArg1 =
{ "MasterMACS port name", iocshArgString };
static const iocshArg
MasterMACSCreateControllerArg2 = { "Number of axes", iocshArgInt };
static const iocshArg *const
MasterMACSCreateControllerArgs[] = { &MasterMACSCreateControllerArg0,
&MasterMACSCreateControllerArg1,
&MasterMACSCreateControllerArg2
};
static const iocshFuncDef
MasterMACSCreateControllerDef =
{ "MasterMACSCreateController", 3, MasterMACSCreateControllerArgs };
static void MasterMACSCreateContollerCallFunc(const iocshArgBuf * args)
{
MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival);
}
/**
* C wrapper for the MasterMACSAxis constructor.
* See MasterMAXSAxis::MasterMACSAxis.
*
*/
asynStatus MasterMACSCreateAxis(const char *MasterMACSPort, /* specify which controller by port name */
int axis)
{ /* axis number (start from 1). */
MasterMACSController *pC;
MasterMACSAxis *pAxis;
static const char *functionName = "MasterMACSCreateAxis";
pC = (MasterMACSController *) findAsynPortDriver(MasterMACSPort);
if (!pC) {
printf("%s:%s: Error port %s not found\n", "MasterMACSDriver",
functionName, MasterMACSPort);
return asynError;
}
pC->lock();
pAxis = new MasterMACSAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/* MasterMACSCreateAxis */
static const iocshArg
MasterMACSCreateAxisArg0 = { "Controller port name", iocshArgString };
static const iocshArg
MasterMACSCreateAxisArg1 = { "Axis number", iocshArgInt };
static const iocshArg *const
MasterMACSCreateAxisArgs[] = { &MasterMACSCreateAxisArg0,
&MasterMACSCreateAxisArg1
};
static const iocshFuncDef
configMasterMACSAxis =
{ "MasterMACSCreateAxis", 2, MasterMACSCreateAxisArgs };
static void configMasterMACSAxisCallFunc(const iocshArgBuf * args)
{
MasterMACSCreateAxis(args[0].sval, args[1].ival);
}
static void MasterMACSRegister(void)
{
iocshRegister(&MasterMACSCreateControllerDef,
MasterMACSCreateContollerCallFunc);
iocshRegister(&configMasterMACSAxis, configMasterMACSAxisCallFunc);
}
extern "C" {
epicsExportRegistrar(MasterMACSRegister);
}

71
sinqEPICSApp/src/MasterMACSDriver.h Normal file
View File

@ -0,0 +1,71 @@
/*
Driver for the MasterMACS motor controllers used at SINQ.
For documentation see the standard manuals area for SINQ or
Marcel Schildt.
The MasterMACS has a special line protocol which is implemented in
drvAsynMMACSPort.c. The driver will not work with a standard asyn IPPort,
only with the special one.
Mark Koennecke, March 2023
*/
#include "SINQController.h"
#include "SINQAxis.h"
#define COMLEN 50
class MasterMACSAxis : public SINQAxis
{
public:
/* These are the methods we override from the base class */
MasterMACSAxis(class MasterMACSController *pC, int axis);
void report(FILE *fp, int level);
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
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 poll(bool *moving);
asynStatus setPosition(double position);
asynStatus setClosedLoop(bool closedLoop);
private:
MasterMACSController *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
* Abbreviated because it is used very frequently */
double position;
int homing;
time_t next_poll;
int errorReported;
int hasStarted; /* The motor status is invalid if the thing has not run once */
int isOn(int axisStatus);
friend class MasterMACSController;
};
#define EnableAxisString "ENABLE_AXIS"
#define AxisEnabledString "AXIS_ENABLED"
class MasterMACSController : public SINQController {
public:
MasterMACSController(const char *portName, const char *MasterMACSPortName, int numAxes);
void report(FILE *fp, int level);
MasterMACSAxis* getAxis(asynUser *pasynUser);
MasterMACSAxis* getAxis(int axisNo);
// overloaded because we want to enable/disable the motor
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
// overloaded because we want to read the axis state
asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
friend class MasterMACSAxis;
private:
asynUser *pasynUserController_;
MasterMACSAxis **pAxes_; /**< Array of pointers to axis objects */
asynStatus transactController(int axis, char command[COMLEN], char reply[COMLEN]);
int enableAxis_;
int axisEnabled_;
};

9
testmmac/db/asynRecord.db Normal file
View File

@ -0,0 +1,9 @@
record(asyn,"$(P)$(R)")
{
field(DTYP,"asynRecordDevice")
field(PORT,"$(PORT)")
field(ADDR,"$(ADDR)")
field(OMAX,"$(OMAX)")
field(IMAX,"$(IMAX)")
}

23
testmmac/db/basic_asyn_motor.db Normal file
View File

@ -0,0 +1,23 @@
record(motor,"$(P)$(M)")
{
field(DESC,"$(DESC)")
field(DTYP,"$(DTYP)")
field(DIR,"$(DIR)")
field(VELO,"$(VELO)")
field(VBAS,"$(VBAS)")
field(ACCL,"$(ACCL)")
field(BDST,"$(BDST)")
field(BVEL,"$(BVEL)")
field(BACC,"$(BACC)")
field(OUT,"@asyn($(PORT),$(ADDR))")
field(MRES,"$(MRES)")
field(PREC,"$(PREC)")
field(EGU,"$(EGU)")
field(DHLM,"$(DHLM)")
field(DLLM,"$(DLLM)")
field(INIT,"$(INIT)")
field(TWV,"1")
field(RDBD,"$(RDBD)")
field(RTRY,"0")
}

21
testmmac/db/basic_motor.db Normal file
View File

@ -0,0 +1,21 @@
grecord(motor,"$(P)$(M)")
{
field(DESC,"$(DESC)")
field(DTYP,"$(DTYP)")
field(DIR,"$(DIR)")
field(VELO,"$(VELO)")
field(VBAS,"$(VBAS)")
field(ACCL,"$(ACCL)")
field(BDST,"$(BDST)")
field(BVEL,"$(BVEL)")
field(BACC,"$(BACC)")
field(OUT,"#C$(C) S$(S) @")
field(MRES,"$(MRES)")
field(PREC,"$(PREC)")
field(EGU,"$(EGU)")
field(DHLM,"$(DHLM)")
field(DLLM,"$(DLLM)")
field(INIT,"$(INIT)")
field(TWV,"1")
}

18
testmmac/db/motorMessage.db Normal file
View File

@ -0,0 +1,18 @@
# The message text
record(waveform, "$(P)$(M)-MsgTxt") {
field(DTYP, "asynOctetRead")
field(INP, "@asyn($(PORT),$(N),1) MOTOR_MESSAGE_TEXT")
field(FTVL, "CHAR")
field(NELM, "80")
field(SCAN, "I/O Intr")
}
record(ao,"$(P)m$(N)-Resolution"){
field(DESC,"m$(N) Resolution")
field(DOL,"$(P)m$(N).MRES CP MS")
field(OMSL,"closed_loop")
field(DTYP,"asynFloat64")
field(OUT,"@asyn($(PORT),$(N))MOTOR_RESOLUTION")
field(PREC,"3")
}

14
testmmac/db/pmacV3.db Normal file
View File

@ -0,0 +1,14 @@
# enable axis
record(longout, "$(P)$(M):Enable") {
field(DTYP, "asynInt32")
field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS")
field(PINI, "YES")
}
# enable axis
record(longin, "$(P)$(M):Enable_RBV") {
field(DTYP, "asynInt32")
field(INP, "@asyn($(PORT),$(N),1) AXIS_ENABLED")
field(PINI, "YES")
field(SCAN, "1 second")
}

47
testmmac/db/sinq_asyn_motor.db Normal file
View File

@ -0,0 +1,47 @@
record(motor,"$(P)$(M)")
{
field(DESC,"$(DESC)")
field(DTYP,"$(DTYP)")
field(DIR,"$(DIR)")
field(VELO,"$(VELO)")
field(VBAS,"$(VBAS)")
field(ACCL,"$(ACCL)")
field(BDST,"$(BDST)")
field(BVEL,"$(BVEL)")
field(BACC,"$(BACC)")
field(OUT,"@asyn($(PORT),$(ADDR))")
field(MRES,"$(MRES)")
field(PREC,"$(PREC)")
field(EGU,"$(EGU)")
field(DHLM,"$(DHLM)")
field(DLLM,"$(DLLM)")
field(INIT,"$(INIT)")
field(TWV,"1")
field(RDBD,"$(RDBD)")
}
# The message text
record(waveform, "$(P)$(M)-MsgTxt") {
field(DTYP, "asynOctetRead")
field(INP, "@asyn($(PORT),$(N),1) MOTOR_MESSAGE_TEXT")
field(FTVL, "CHAR")
field(NELM, "80")
field(SCAN, "I/O Intr")
}
# enable axis
record(longout, "$(P)$(M):Enable") {
field(DTYP, "asynInt32")
field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS")
field(PINI, "YES")
}
# enable axis
record(longin, "$(P)$(M):Enable_RBV") {
field(DTYP, "asynInt32")
field(INP, "@asyn($(PORT),$(N),1) AXIS_ENABLED")
field(PINI, "YES")
field(SCAN, "5 second")
}

15
testmmac/mmacs.sub Normal file
View File

@ -0,0 +1,15 @@
file "$(TOP)/db/basic_asyn_motor.db"
{
pattern
{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DLLM, DHLM, INIT}
{KM36:mota:, 5, "m$(N)", "asynMotor", mota, 5, "m5", degree, Pos, 2.0, 0.1, .2, 0, 1, .2, 1., 3, -20.0, 20.0, ""}
{KM36:mota:, 6, "m$(N)", "asynMotor", mota, 6, "m6", degree, Pos, 2.0, 0.1, .2, 0, 1, .2, 1., 3, -20.0, 20.0, ""}
}
file "$(TOP)/db/motorMessage.db"
{
pattern
{P,N, M,PORT}
{KM36:mota:, 5, "m$(N)",mota}
{KM36:mota:, 6, "m$(N)",mota}
}

8
testmmac/mmacs2.sub Normal file
View File

@ -0,0 +1,8 @@
file "$(TOP)/db/sinq_asyn_motor.db"
{
pattern
{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DLLM, DHLM, RDBD, INIT}
{KM36:mota:,5,"m$(N)","asynMotor", mota, 5, "m5", degree, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3,-400000.000,400000.000,.01, ""}
{KM36:mota:,6,"m$(N)","asynMotor", mota, 6, "m6", degree, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3,-200000.000,200000.000,.01, ""}
}

13
testmmac/mota.sub Normal file
View File

@ -0,0 +1,13 @@
file "$(TOP)/db/basic_asyn_motor.db"
{
pattern
{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DLLM, DHLM, INIT}
{KM36:mota:, 2, "m$(N)", "asynMotor", mota, 2, "sgl", degree, Pos, 2.0, 0.1, .2, 0, 1, .2, 1., 3, -20.0, 20.0, ""}
}
file "$(TOP)/db/motorMessage.db"
{
pattern
{P,N, M,PORT}
{KM36:mota:, 2, "m$(N)",mota}
}

12
testmmac/motaspeed.sub Normal file
View File

@ -0,0 +1,12 @@
file "$(TOP)/db/sinq_asyn_motor.db"
{
pattern
{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DLLM, DHLM, INIT, RDBD}
{KM36:mota:,2,"m$(N)","asynMotor", mota, 2, "sgl", degree, Pos, 100, 0.1, .2, 0, 1, .2, 1., 3,-180.0,360.0,"", 0.2}}
file "$(TOP)/db/motorMessage.db"
{
pattern
{P,N, M,PORT}
{KM36:mota:, 2, "m$(N)",mota}
}

19
testmmac/testmmacs.cmd Executable file
View File

@ -0,0 +1,19 @@
#!/usr/local/bin/iocsh
require sinq,koennecke
epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/testmmac")
#drvAsynMMACSPortConfigure("macs1", "localhost:8080",0,0,0)
#asynInterposeEosConfig("macs1", 0, 0, 0)
#dbLoadRecords("$(TOP)/db/asynRecord.db","P=KM36:,R=macs1,PORT=macs1,ADDR=0,OMAX=80,IMAX=80")
#asynSetTraceMask("macs1", 0, 255)
drvAsynIPPortConfigure("macs1", "localhost:8080",0,0,0)
#asynSetTraceMask("macs1", 0, 255)
MasterMACSCreateController("mota","macs1",7)
MasterMACSCreateAxis("mota",5)
MasterMACSCreateAxis("mota",6)
dbLoadTemplate "mmacs2.sub"

9
testpmacV3/db/asynRecord.db Normal file
View File

@ -0,0 +1,9 @@
record(asyn,"$(P)$(R)")
{
field(DTYP,"asynRecordDevice")
field(PORT,"$(PORT)")
field(ADDR,"$(ADDR)")
field(OMAX,"$(OMAX)")
field(IMAX,"$(IMAX)")
}

22
testpmacV3/db/basic_asyn_motor.db Normal file
View File

@ -0,0 +1,22 @@
record(motor,"$(P)$(M)")
{
field(DESC,"$(DESC)")
field(DTYP,"$(DTYP)")
field(DIR,"$(DIR)")
field(VELO,"$(VELO)")
field(VBAS,"$(VBAS)")
field(ACCL,"$(ACCL)")
field(BDST,"$(BDST)")
field(BVEL,"$(BVEL)")
field(BACC,"$(BACC)")
field(OUT,"@asyn($(PORT),$(ADDR))")
field(MRES,"$(MRES)")
field(PREC,"$(PREC)")
field(EGU,"$(EGU)")
field(DHLM,"$(DHLM)")
field(DLLM,"$(DLLM)")
field(INIT,"$(INIT)")
field(TWV,"1")
field(RTRY,"0")
}

19544
testpmacV3/db/huber.dbd Normal file
View File

File diff suppressed because it is too large Load Diff

9
testpmacV3/db/motorMessage.db Normal file
View File

@ -0,0 +1,9 @@
# The message text
record(waveform, "$(P)$(M)-MsgTxt") {
field(DTYP, "asynOctetRead")
field(INP, "@asyn($(PORT),$(N),1) MOTOR_MESSAGE_TEXT")
field(FTVL, "CHAR")
field(NELM, "80")
field(SCAN, "I/O Intr")
}

14
testpmacV3/db/pmacV3.db Normal file
View File

@ -0,0 +1,14 @@
# enable axis
record(longout, "$(P)$(M):Enable") {
field(DTYP, "asynInt32")
field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS")
field(PINI, "YES")
}
# enable axis
record(longin, "$(P)$(M):Enable_RBV") {
field(DTYP, "asynInt32")
field(INP, "@asyn($(PORT),$(N),1) AXIS_ENABLED")
field(PINI, "YES")
field(SCAN, "1 second")
}

27
testpmacV3/mcu.substitutions Normal file
View File

@ -0,0 +1,27 @@
file "$(TOP)/db/basic_asyn_motor.db"
{
pattern
{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT}
{SQ:TEST:mcu:, 2, "m$(N)", "asynMotor", mcu, 2, "m$(N)", degrees, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 358., 2., ""}
{SQ:TEST:mcu:, 6, "m$(N)", "asynMotor", mcu, 6, "m$(N)", degrees, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 358., 2., ""}
{SQ:TEST:mcu:,10, "m$(N)", "asynMotor", mcu,10, "m$(N)", degrees, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 358., 2., ""}
}
file "$(TOP)/db/motorMessage.db"
{
pattern
{P,N, M,PORT}
{SQ:TEST:mcu:, 2, "m$(N)", mcu}
{SQ:TEST:mcu:, 6, "m$(N)", mcu}
{SQ:TEST:mcu:,10, "m$(N)", mcu}
}
file "$(TOP)/db/pmacV3.db"
{
pattern
{P,N, M,PORT}
{SQ:TEST:mcu:, 2, "m$(N)", mcu}
{SQ:TEST:mcu:, 6, "m$(N)", mcu}
{SQ:TEST:mcu:,10, "m$(N)", mcu}
}

25
testpmacV3/st.cmd Executable file
View File

@ -0,0 +1,25 @@
#!/ioc/tools/iocsh
require sinq,brambilla_m
#require sinq,koennecke
epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/testpmacV3")
epicsEnvSet("EPICS_CA_ADDR_LIST","127.0.0.1")
epicsEnvSet("STREAM_PROTOCOL_PATH","$(TOP)/db")
var streamDebug 3
# motors
pmacAsynIPConfigure("pmacV3", "129.129.138.234:1025")
pmacV3CreateController("mcu","pmacV3",0,16,50,1000);
pmacV3CreateAxis("mcu",2,0);
pmacV3CreateAxis("mcu",6,0);
pmacV3CreateAxis("mcu",10,0);
dbLoadTemplate "$(TOP)/mcu.substitutions"
dbLoadRecords("$(TOP)/db/asynRecord.db","P=SQ:TEST:,R=mcu,PORT=pmacV3,ADDR=0,OMAX=80,IMAX=80")
iocInit

53
utils/macmaster.py Normal file
View File

@ -0,0 +1,53 @@
"""
Usage: macmaster.py macmasterhost port
Listen for commands to send and returns reponse
until exit has been typed
Mark Koennecke, March 2023
"""
import socket
import struct
class MasterMACS():
def __init__(self, host, port):
self._socke = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._socke.connect((host, int(port)))
def send(self, command):
buf = struct.pack('BBBB', 0x05, len(command) + 4, 0, 0x19)
buf += bytes(command, 'utf-8')
buf += struct.pack('BB', 0x0D, 0x03)
self._socke.send(buf)
def receive(self):
buf = self._socke.recv(35, socket.MSG_WAITALL)
if len(buf) < 35:
raise EOFException('Master MACS returned only %d bytes, 30 expected' % len(buf))
idx = buf.find(0x0D)
ackbyte = buf[idx-1]
if ackbyte == 0x06:
ack = 'ACK'
elif ackbyte == 0x15:
ack = 'NAK',
else:
ack = 'NO'
reply = buf[4:idx-1].decode('utf-8')
return ack, reply
if __name__ == "__main__":
from sys import argv, exit, stdin
if len(argv) < 3:
print('Usage:\n\tmacmaster.py machost macport')
exit(1)
mac = MasterMACS(argv[1], argv[2])
while(True):
# import pdb; pdb.set_trace()
line = stdin.readline()
if line.find('exit') >= 0:
exit(0)
mac.send(line.strip())
ack, reply = mac.receive()
print('%s, %s' %(ack, reply))

90
utils/syncMasterMAC.py Executable file
View File

@ -0,0 +1,90 @@
#!/usr/bin/env python3
"""
This little program synchronizes an EPICS substitutions file with values read
from a MasterMAC motor controller.
Mark Koennecke, February 2023
"""
import sys
from macmaster import MasterMACS
def find_commas(rawline):
comma_list = []
for i in range(len(rawline)):
if rawline[i] == ',':
comma_list.append(i)
return comma_list
def pretty_line(newlist, comma_list):
"""
adds spaces to match the tabbing of the patter line
"""
newline = ''
for item, idx in zip(newlist, comma_list):
length = len(newline) + 1 + len(item)
if length < idx:
newline += ' ' * (idx - length)
newline += item
newline += ','
newline += newlist[-1]
return newline[0:-1]
def transact(command):
global mac
mac.send(command)
return mac.receive()
def fix_line(par_list, index_list):
# import pdb; pdb.set_trace()
addridx = index_list.index('ADDR')
motNo = int(par_list[addridx])
ack, reply = transact('%dR24' % motNo)
idx = reply.find('=')
lowlim = reply[idx+1:]
lowidx = index_list.index('DLLM')
par_list[lowidx] = lowlim.strip()
ack, reply = transact('%dR23' % motNo)
idx = reply.find('=')
highlim = reply[idx+1:]
highidx = index_list.index('DHLM')
par_list[highidx] = highlim.strip()
# speed = transact('Q%d03' % motNo)
# speedidx = index_list['VELO']
# par_list[speedidx] = speed.trim()
return par_list
def scan_substitution_file(filename):
index_list = None
# import pdb; pdb.set_trace()
with open(filename, 'r') as fin:
rawline = fin.readline()
while rawline:
line = rawline.replace(' ','')
line = line.strip('{}')
l = line.split(',')
if line.find('DHLM') > 0:
index_list = l
comma_list = find_commas(rawline)
sys.stdout.write(rawline)
elif not index_list:
sys.stdout.write(rawline)
else:
if len(l) == len(index_list):
newlist = fix_line(l, index_list)
# newline = ','.join(newlist)
newline = pretty_line(newlist, comma_list)
sys.stdout.write('{' + newline + '\n')
else:
sys.stdout.write(rawline)
rawline = fin.readline()
#------------------ main
if len(sys.argv) < 4:
print('Usage:\n\tsyncMMACSub.py <host> <port> <substitutions-file>\n')
sys.exit(1)
mac = MasterMACS(sys.argv[1], sys.argv[2])
scan_substitution_file(sys.argv[3])