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

Changes in pmac controller

Browse Source
This commit is contained in:
michele-brambilla
2020-02-17 11:10:02 +01:00
parent 12249d5471
commit e9a615d0fa
5 changed files with 387 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
sinqEPICSApp/src/pmacController.cpp
View File

@@ -39,6 +39,7 @@ using std::endl;
#include <iocsh.h>
#include <drvSup.h>
#include <registryFunction.h>
#include <errlog.h>
#include "asynOctetSyncIO.h"
@@ -130,6 +131,8 @@ const epicsUInt32 pmacController::PMAX_AXIS_GENERAL_PROB2 = (PMAC_STATUS2_DESIRE
extern "C" {
asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod);
asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod);
asynStatus pmacCreateAxis(const char *pmacName, int axis);
asynStatus pmacCreateAxis(const char *pmacName, int numAxis);
@@ -154,7 +157,7 @@ pmacController::pmacController(const char *portName, const char *lowLevelPortNam
createParam(PMAC_C_CommsErrorString, asynParamInt32, &PMAC_C_CommsError_);
// Connect our Asyn user to the low level port that is a parameter to this constructor
if (lowLevelPortConnect(lowLevelPortName, lowLevelPortAddress, &lowLevelPortUser_, "\006", "\r") != asynSuccess) {
if (lowLevelPortConnect(lowLevelPortName, lowLevelPortAddress, &lowLevelPortUser_, "\006", (char *)"\r") != asynSuccess) {
printf("%s: Failed to connect to low level asynOctetSyncIO port %s\n", functionName, lowLevelPortName);
setIntegerParam(PMAC_C_CommsError_, 1);
} else {
@@ -344,8 +347,6 @@ asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
pmacAxis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
double encRatio = 1.0;
epicsInt32 encposition = 0;
char message[132];
static const char *functionName = "pmacController::writeFloat64";
@@ -532,6 +533,18 @@ asynStatus pmacCreateController(const char *portName, const char *lowLevelPortNa
return asynSuccess;
}
asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
int numAxes, int movingPollPeriod, int idlePollPeriod)
{
SeleneController *ppmacController
= new SeleneController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;
return asynSuccess;
}
/**
* C wrapper for the pmacAxis constructor.
* See pmacAxis::pmacAxis.
@@ -585,6 +598,61 @@ asynStatus pmacCreateHRPTAxis(const char *pmacName, /* specify which con
return asynSuccess;
}
/**
* C wrapper for the SeleneAxis constructor.
* See SeleneAxis::SeleneAxis.
*
*/
asynStatus SeleneCreateAxis(const char *pmacName, /* specify which controller by port name */
int axis, /* axis number (start from 1). */
double limitTarget)
{
SeleneController *pC;
SeleneAxis *pAxis;
static const char *functionName = "SeleneCreateAxis";
pC = (SeleneController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new SeleneAxis(pC, axis, limitTarget);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the Selene LiftAxis constructor.
* See LiftAxis::LiftAxis.
*
*/
asynStatus LiftCreateAxis(const char *pmacName, /* specify which controller by port name */
int axis) /* axis number (start from 1). */
{
pmacController *pC;
LiftAxis *pAxis;
static const char *functionName = "LiftCreateAxis";
pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
driverName, functionName, pmacName);
return asynError;
}
pC->lock();
pAxis = new LiftAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C Wrapper function for pmacHRPTAxis constructor.
* See pmacAxis::pmacAxis.
@@ -617,6 +685,61 @@ asynStatus pmacCreateAxes(const char *pmacName,
return asynSuccess;
}
/*================================ SeleneController ===============================================*/
asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
SeleneAxis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
char message[132];
static const char *functionName = "SeleneController::writeFloat64";
sprintf(message,"%s, reason %d", functionName, function);
pAxis = (SeleneAxis *)this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}
/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);
// TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command
if (function == motorLowLimit_) {
sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Setting low limit on controller %s, axis %d to %f\n",
functionName, portName, pAxis->axisNo_, value);
errlogPrintf("Setting low limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
} else if (function == motorHighLimit_) {
sprintf(command, "Q%d53=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
"%s: Setting high limit on controller %s, axis %d to %f\n",
functionName, portName, pAxis->axisNo_, value);
errlogPrintf("Setting high limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
}
//Execute the command.
if (command[0] != 0 && status == asynSuccess) {
status = lowLevelWriteRead(pAxis->axisNo_,command, response);
}
//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeFloat64(pasynUser, value);
return status;
}
/* Code for iocsh registration */
#ifdef vxWorks
@@ -641,6 +764,12 @@ static void configpmacCreateControllerCallFunc(const iocshArgBuf *args)
pmacCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
static const iocshFuncDef configSeleneCreateController = {"SeleneCreateController", 6, pmacCreateControllerArgs};
static void configSeleneCreateControllerCallFunc(const iocshArgBuf *args)
{
SeleneCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
/* pmacCreateAxis */
static const iocshArg pmacCreateAxisArg0 = {"Controller port name", iocshArgString};
@@ -666,6 +795,34 @@ static void configpmacHRPTAxisCallFunc(const iocshArgBuf *args)
pmacCreateHRPTAxis(args[0].sval, args[1].ival);
}
/* SeleneCreateAxis */
static const iocshArg SeleneCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg SeleneCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg SeleneCreateAxisArg2 = {"limitTraget", iocshArgDouble};
static const iocshArg * const SeleneCreateAxisArgs[] = {&SeleneCreateAxisArg0,
&SeleneCreateAxisArg1,
&SeleneCreateAxisArg2};
static const iocshFuncDef configSeleneCreateAxis = {"SeleneCreateAxis", 3, SeleneCreateAxisArgs};
static void configSeleneCreateAxisCallFunc(const iocshArgBuf *args)
{
SeleneCreateAxis(args[0].sval, args[1].ival,args[2].dval);
}
/* LiftCreateAxis */
static const iocshArg LiftCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg LiftCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const LiftCreateAxisArgs[] = {&LiftCreateAxisArg0,
&LiftCreateAxisArg1};
static const iocshFuncDef configLiftAxis = {"LiftCreateAxis", 2, LiftCreateAxisArgs};
static void configLiftAxisCallFunc(const iocshArgBuf *args)
{
LiftCreateAxis(args[0].sval, args[1].ival);
}
/* pmacCreateAxes */
static const iocshArg pmacCreateAxesArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAxesArg1 = {"Num Axes", iocshArgInt};
@@ -683,8 +840,11 @@ static void configpmacAxesCallFunc(const iocshArgBuf *args)
static void pmacControllerRegister(void)
{
iocshRegister(&configpmacCreateController, configpmacCreateControllerCallFunc);
iocshRegister(&configSeleneCreateController, configSeleneCreateControllerCallFunc);
iocshRegister(&configpmacAxis, configpmacAxisCallFunc);
iocshRegister(&configpmacHRPTAxis, configpmacHRPTAxisCallFunc);
iocshRegister(&configSeleneCreateAxis, configSeleneCreateAxisCallFunc);
iocshRegister(&configLiftAxis, configLiftAxisCallFunc);
iocshRegister(&configpmacAxes, configpmacAxesCallFunc);
}
epicsExportRegistrar(pmacControllerRegister);
@@ -693,3 +853,4 @@ epicsExportRegistrar(pmacControllerRegister);
} // extern "C"