From 3b7133ecfe67a8ae9804aea73da3c3de6456f307 Mon Sep 17 00:00:00 2001 From: michele-brambilla Date: Tue, 18 Feb 2020 10:00:30 +0100 Subject: [PATCH 1/4] Make sure that the SINQ library version stays the same --- Makefile.RHEL7 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile.RHEL7 b/Makefile.RHEL7 index bdd2198..9875c20 100644 --- a/Makefile.RHEL7 +++ b/Makefile.RHEL7 @@ -2,7 +2,7 @@ include /ioc/tools/driver.makefile MODULE=sinq - +LIBVERSION=koennecke BUILDCLASSES=Linux # additional module dependencies From c4fe45c0cb9699c6a755c2ba0ec38f6f2d9fc653 Mon Sep 17 00:00:00 2001 From: michele-brambilla Date: Mon, 17 Feb 2020 11:10:02 +0100 Subject: [PATCH 2/4] Add a set PV for the SeleneMotor - make the number of extra parameters in SINQController configurable - add database entry for the motor set field - add the new function "setMotorPosition_" to the Selene controller - execute Qx59= to set the new position --- configure/RELEASE | 25 ++-- sinqEPICSApp/Db/motorSet.db | 6 + sinqEPICSApp/src/SINQController.cpp | 4 +- sinqEPICSApp/src/SINQController.h | 2 +- sinqEPICSApp/src/pmacAxis.cpp | 130 ++++++++++++++++++- sinqEPICSApp/src/pmacAxis.h | 49 +++++++ sinqEPICSApp/src/pmacController.cpp | 190 +++++++++++++++++++++++++++- sinqEPICSApp/src/pmacController.h | 32 ++++- 8 files changed, 411 insertions(+), 27 deletions(-) create mode 100644 sinqEPICSApp/Db/motorSet.db diff --git a/configure/RELEASE b/configure/RELEASE index 41fe199..118ef2c 100644 --- a/configure/RELEASE +++ b/configure/RELEASE @@ -25,17 +25,20 @@ TEMPLATE_TOP=$(EPICS_BASE)/templates/makeBaseApp/top #SNCSEQ=$(EPICS_BASE)/../modules/soft/seq # EPICS_BASE usually appears last so other apps can override stuff: -EPICS_BASE=/opt/epics/bases/base-3.14.12.5 + +#EPICS_BASE= +#MODULES=/home/epics/modules # Set RULES here if you want to take build rules from somewhere # other than EPICS_BASE: -#RULES=/path/to/epics/support/module/rules/x-y -MOTOR=/opt/epics/modules/motor/6.10.0/3.14.12.5 -ASYN=/opt/epics/modules/asyn/4.27.0/3.14.12.5 -SYNAPPSSTD=/opt/epics/modules/synAppsStd/3.4.1/3.14.12.5/ -#ANC=/usr/local/epics/anc350v17 -STREAMS=/opt/epics/modules/streamdevice/2.6.0/3.14.12.5 -#LAKESHORE336=/usr/local/epics/support/lakeshore336 -BUSY=/opt/epics/modules/busy/1.6.0/3.14.12.5 -#OXINSTCRYOJET=/usr/local/epics/support/OxInstCryojet-2-18-3 -PCRE=/opt/epics/modules/pcre/8.36.0/3.14.12.5 +##RULES=/path/to/epics/support/module/rules/x-y +#MOTOR=/opt/epics/modules/motor/6.10.0/3.14.12.5 +#ASYN=/opt/epics/modules/asyn/4.27.0/3.14.12.5 +#SYNAPPSSTD=/opt/epics/modules/synAppsStd/3.4.1/3.14.12.5/ +##ANC=/usr/local/epics/anc350v17 +#STREAMS=/opt/epics/modules/streamdevice/2.6.0/3.14.12.5 +##LAKESHORE336=/usr/local/epics/support/lakeshore336 +#BUSY=/opt/epics/modules/busy/1.6.0/3.14.12.5 +##OXINSTCRYOJET=/usr/local/epics/support/OxInstCryojet-2-18-3 +#PCRE=/opt/epics/modules/pcre/8.36.0/3.14.12.5 + diff --git a/sinqEPICSApp/Db/motorSet.db b/sinqEPICSApp/Db/motorSet.db new file mode 100644 index 0000000..91d7b80 --- /dev/null +++ b/sinqEPICSApp/Db/motorSet.db @@ -0,0 +1,6 @@ +# workaround over set position +record(ao, "$(P)$(M)-SetPosition") { + field(DTYP, "asynFloat64") + field(OUT, "@asyn($(PORT),$(N),1) SET_MOTOR_POSITION") + field(PINI, "YES") +} diff --git a/sinqEPICSApp/src/SINQController.cpp b/sinqEPICSApp/src/SINQController.cpp index afbe3ef..4070156 100644 --- a/sinqEPICSApp/src/SINQController.cpp +++ b/sinqEPICSApp/src/SINQController.cpp @@ -10,8 +10,8 @@ #include "SINQController.h" #include "asynMotorController.h" -SINQController::SINQController(const char *portName, const char *SINQPortName, int numAxes) - : asynMotorController(portName, numAxes+1, NUM_MOTOR_DRIVER_PARAMS+2, +SINQController::SINQController(const char *portName, const char *SINQPortName, int numAxes, const int& extraParams) + : asynMotorController(portName, numAxes+1, NUM_MOTOR_DRIVER_PARAMS+extraParams, 0, // No additional interfaces beyond those in base class 0, // No additional callback interfaces beyond those in base class ASYN_CANBLOCK | ASYN_MULTIDEVICE, diff --git a/sinqEPICSApp/src/SINQController.h b/sinqEPICSApp/src/SINQController.h index 15850e3..27cdd62 100644 --- a/sinqEPICSApp/src/SINQController.h +++ b/sinqEPICSApp/src/SINQController.h @@ -17,7 +17,7 @@ class epicsShareClass SINQController : public asynMotorController { public: - SINQController(const char *portName, const char *SINQPortName, int numAxes); + SINQController(const char *portName, const char *SINQPortName, int numAxes, const int& extraParams=2); friend class SINQAxis; protected: diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 52c0060..c7a5d7f 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -8,9 +8,9 @@ * 23 May 2012 * * Substantially modified for use at SINQ at PSI. - * The thing with the PMACS is that they can be programmed. + * The thing with the PMAC's is that they can be programmed. * This affects also the commands they understand. - * Our PMACS also do not seem to like to return multiple replies..... + * Our PMAC's also do not seem to like to return multiple replies..... * * I use a starting flag to catch a peculiar feature of our PMAC implementation: * when the motor is hung, it wont start. I check for this and cause an alarm. @@ -44,6 +44,8 @@ #define MULT 1000. +#define ABS(x) (x < 0 ? -(x) : (x)) + extern "C" void shutdownCallback(void *pPvt) { pmacController *pC = static_cast(pPvt); @@ -244,7 +246,8 @@ asynStatus pmacAxis::setPosition(double position) { //int status = 0; static const char *functionName = "pmacAxis::setPosition"; - + errlogPrintf("executing : %s\n", functionName); + pC_->debugFlow(functionName); // Cannot do this. @@ -417,6 +420,8 @@ asynStatus pmacAxis::getAxisStatus(bool *moving) previous_position_ = position; previous_direction_ = direction; + // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, axErr, position); + /* are we done? */ if((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0 ){ done = 1; @@ -561,3 +566,122 @@ asynStatus pmacHRPTAxis::getAxisStatus(bool *moving) } return result; } + +/*================================= SeleneAxis code ======================================================*/ +asynStatus SeleneAxis::home(double min_velocity, double max_velocity, double acceleration, int forwards) +{ + asynStatus status = asynError; + static const char *functionName = "SeleneAxis::home"; + + pC_->debugFlow(functionName); + + updateMsgTxtFromDriver("Cannot home on this axis type"); + + + return status; +} +/*----------------------------------------------------------------------------------------------------------------*/ +asynStatus SeleneAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration) +{ + asynStatus status = asynError; + static const char *functionName = "SeleneAxis::move"; + double realPosition; + + updateMsgTxtFromDriver(""); + + pC_->debugFlow(functionName); + + char command[128] = {0}; + char response[32] = {0}; + + pC_->debugFlow(functionName); + + if(relative){ + realPosition = previous_position_ + position/MULT; + } else { + realPosition = position/MULT; + } + startTime = time(NULL); + status6Time = 0; + starting = 1; + + /* + Run into limit when asked for by a suitable position, else + position absolutely + */ + if(ABS(realPosition - limitTarget) < .05){ + sprintf(command,"P%d50=3", axisNo_); + } else { + sprintf(command,"Q%d51=%f P%x50=1",axisNo_, realPosition, axisNo_); + } + + errlogPrintf("Sending drive command: %s\n", command); + + status = pC_->lowLevelWriteRead(axisNo_,command, response); + + return status; +} +/*----------------------------------------------------------------------------------------------------*/ +asynStatus SeleneAxis::setPosition(double position) { + asynStatus status = asynError; + return status; +} +/*======================================= Selene Lift Axis Code ===========================================*/ +asynStatus LiftAxis::move(double position, int relative, double min_velocity, + double max_velocity, double acceleration) +{ + asynStatus status = asynError; + static const char *functionName = "LiftAxis::move"; + double realPosition; + + updateMsgTxtFromDriver(""); + pC_->debugFlow(functionName); + + char command[128] = {0}; + char response[32] = {0}; + + pC_->debugFlow(functionName); + + if(relative){ + realPosition = previous_position_ + position/MULT; + } else { + realPosition = position/MULT; + } + startTime = time(NULL); + status6Time = 0; + starting = 1; + + sprintf( command, "Q15%d=%12.4f", axisNo_, realPosition); + + status = pC_->lowLevelWriteRead(axisNo_,command, response); + waitStart = 1; + + return status; +} +/*-------------------------------------------------------------------------------------------------------- + Return *moving until the motor moved started moving. This enables the start command + to be sent separatly. +----------------------------------------------------------------------------------------------------------*/ +asynStatus LiftAxis::poll(bool *moving) +{ + asynStatus status; + + status = getAxisStatus(moving); + if(*moving == false && waitStart == 1){ + *moving = true; + setIntegerParam(pC_->motorStatusMoving_, true); + setIntegerParam(pC_->motorStatusDone_, false); + } + if(*moving){ + waitStart = 0; + } + callParamCallbacks(); + return status; +} +/*--------------------------------------------------------------------------------------------------------------*/ +asynStatus LiftAxis::stop(double acceleration) +{ + waitStart = 0; + return pmacAxis::stop(acceleration); +} +/*-------------------------------------------------------------------------------------------------------------*/ diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index bb995eb..824f91d 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -10,6 +10,9 @@ * Modified to use the MsgTxt field for SINQ * * Mark Koennecke, January 2019 + * + * EXtended with special motor axis for the Selene + * guide, Mark Koennecke, February 2020 ********************************************/ #ifndef pmacAxis_H @@ -19,6 +22,7 @@ #include "SINQAxis.h" class pmacController; +class SeleneController; class pmacAxis : public SINQAxis { @@ -76,4 +80,49 @@ class pmacHRPTAxis : public pmacAxis }; +class SeleneAxis : public pmacAxis +{ + + public: + SeleneAxis(SeleneController *pController, int axisNo, double limitTarget) : pmacAxis((pmacController *)pController,axisNo) + { + this->limitTarget = limitTarget; + }; + asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); + asynStatus setPosition(double position); + asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); + + protected: + friend class SeleneController; + friend class pmacController; + + private: + double limitTarget; + +}; + +/* + Yet another special set of motors for the Selene Guide at AMOR. Each segment can be lifted or tilted. This is + two motors. One acts as a slave and only writes a new target, the other also sets a new target and sends the + actual movement command. Both motors are coordianted in the motor controller in order to avoid tension on + the guide elements. This gaves rise to the function code LIFTSLAVE and LIFTMASTER. + + In another mode the whole guide can be lifted or tilted. Then motor 1 and 6 get new values and one of them + sends the drive command. This causes all 6 motors to drive synchronously to their new targets. This is + implemented through the LIFTSEGMENT function code. + + Mark Koennecke, February 2020 + + */ +class LiftAxis : public pmacAxis +{ + public: + LiftAxis(pmacController *pController, int axisNo) : pmacAxis((pmacController *)pController,axisNo) {}; + asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); + asynStatus poll(bool *moving); + asynStatus stop(double acceleration); + private: + int waitStart; +}; + #endif /* pmacAxis_H */ diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 39d5434..8f299f3 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -39,6 +39,7 @@ using std::endl; #include #include #include +#include #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); @@ -137,8 +140,8 @@ extern "C" { } pmacController::pmacController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, - int numAxes, double movingPollPeriod, double idlePollPeriod) - : SINQController(portName, lowLevelPortName, numAxes+1) + int numAxes, double movingPollPeriod, double idlePollPeriod, const int& extraParams) + : SINQController(portName, lowLevelPortName, numAxes+1, extraParams) { static const char *functionName = "pmacController::pmacController"; @@ -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,31 @@ 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; +} + + + SeleneController::SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, + int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName, + lowLevelPortName, + lowLevelPortAddress, + numAxes, + movingPollPeriod, + idlePollPeriod, 3) { + static const char *functionName = "seleneController::seleneController"; + createParam(MotorSetPositionString, asynParamFloat64, &setMotorPosition_); + callParamCallbacks(); + } + + /** * C wrapper for the pmacAxis constructor. * See pmacAxis::pmacAxis. @@ -585,6 +611,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 +698,67 @@ 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); + } else if(function == setMotorPosition_){ + snprintf(command,sizeof(command),"Q%d59=%f", pAxis->axisNo_, value); + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Defining position of axis%d to %f\n", + functionName, pAxis->axisNo_, value); + errlogPrintf("Defining position 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 +783,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 +814,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 +859,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 +872,4 @@ epicsExportRegistrar(pmacControllerRegister); } // extern "C" + diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h index d794f2c..2f67fcf 100644 --- a/sinqEPICSApp/src/pmacController.h +++ b/sinqEPICSApp/src/pmacController.h @@ -26,8 +26,8 @@ class pmacController : public SINQController { public: - pmacController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, int numAxes, double movingPollPeriod, - double idlePollPeriod); + pmacController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, int numAxes, double movingPollPeriod, + double idlePollPeriod, const int& extraParams=2); virtual ~pmacController(); @@ -49,13 +49,13 @@ class pmacController : public SINQController { int PMAC_C_GlobalStatus_; int PMAC_C_CommsError_; #define LAST_PMAC_PARAM PMAC_C_CommsError__ + void debugFlow(const char *message); + asynStatus lowLevelWriteRead(int axisNo, const char *command, char *response); private: asynUser* lowLevelPortUser_; epicsUInt32 debugFlag_; - asynStatus lowLevelWriteRead(int axisNo, const char *command, char *response); int lowLevelPortConnect(const char *port, int addr, asynUser **ppasynUser, char *inputEos, char *outputEos); - void debugFlow(const char *message); //static class data members @@ -137,9 +137,31 @@ class pmacController : public SINQController { friend class pmacAxis; friend class pmacHRPTAxis; - + friend class SeleneAxis; + friend class LiftAxis; }; #define NUM_PMAC_PARAMS (&LAST_PMAC_PARAM - &FIRST_PMAC_PARAM + 1) +#define MotorSetPositionString "SET_MOTOR_POSITION" + +class SeleneController : public pmacController { + public: + SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, + int numAxes, double movingPollPeriod, double idlePollPeriod); + + ~SeleneController(void) { } + + // overloaded because we have a different command to set the limits + asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); + + friend class SeleneAxis; + friend class pmacAxis; + + protected: + int setMotorPosition_; + +}; + + #endif /* pmacController_H */ From a25f8cabb9adc15c7418b1bd4324abe768da2ec5 Mon Sep 17 00:00:00 2001 From: michele-brambilla Date: Wed, 19 Feb 2020 15:39:23 +0100 Subject: [PATCH 3/4] Selene positions do not need factor *1000 --- sinqEPICSApp/src/pmacController.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 8f299f3..a4975ed 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -726,13 +726,15 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu // 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)); + sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/MULT)); + // 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)); + sprintf(command, "Q%d53=%d", pAxis->axisNo_, (int)(value/MULT)); + // 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); From f56ef2c74cb9d98b47c58c58067c03c653c08516 Mon Sep 17 00:00:00 2001 From: koennecke Date: Wed, 18 Mar 2020 15:53:44 +0100 Subject: [PATCH 4/4] Committing the last status of selene work in the corona lockdown. The problem is in the MCU, probably --- .gitignore | 121 ++++++++++++++++++++++++++++ 10-llbDevices.rules | 1 + sinqEPICSApp/src/pmacAxis.cpp | 68 ++++++++++++---- sinqEPICSApp/src/pmacAxis.h | 14 +++- sinqEPICSApp/src/pmacController.cpp | 4 +- testEuroMoveUsb.py | 88 ++++++++++++++++++++ 6 files changed, 277 insertions(+), 19 deletions(-) create mode 100644 .gitignore create mode 100644 10-llbDevices.rules create mode 100755 testEuroMoveUsb.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b205e48 --- /dev/null +++ b/.gitignore @@ -0,0 +1,121 @@ +# Took these from the https://github.com/github/gitignore project on October 21, 2011 + +# **** 'Personal' entries don't belong in here - put them in your .git/info/exclude file **** + +# Ignore text editor (e.g. emacs) autosave files +*~ + + +# Compiled Object files +*.slo +*.lo +*.o +*.obj +*.d +SICServer* + +# Compiled Dynamic libraries +*.so + +# Compiled Static libraries +*.lai +*.la +*.a + +# Compiled python files +*.py[co] + +# Eclipse-generated files +*.pydevproject +.project +.metadata +bin/** +tmp/** +tmp/**/* +*.tmp +*.bak +*.swp +*~.nib +local.properties +.classpath +.settings/ +.loadpath + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +*.sln +*.vcproj +*.exe +*.vcxproj +*.filters + +# User-specific files +*.suo +*.user +*.sln.docstates +*.sdf + +#Test results +*.log + +# Build results +[Dd]ebug/ +[Rr]elease/ +*_i.c +*_p.c +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.vspscc +.builds + + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf + +# Visual Studio profiler +*.psess +*.vsp + +# ReSharper is a .NET coding add-in +_ReSharper* + + + +# Others +*.autosave + + +# Windows image file caches +Thumbs.db + + +# Mac OS X Finder +.DS_Store +._* diff --git a/10-llbDevices.rules b/10-llbDevices.rules new file mode 100644 index 0000000..c939cfd --- /dev/null +++ b/10-llbDevices.rules @@ -0,0 +1 @@ +SUBSYSTEM=="usb", ATTR{idVendor}=="04b4", ATTR{idProduct}=="1002", MODE="0666" GROUP="plugdev", TAG+="uaccess" diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index c7a5d7f..e11c3ba 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -44,6 +44,9 @@ #define MULT 1000. +#define IDLEPOLL 2. +#define BUSYPOLL .05 + #define ABS(x) (x < 0 ? -(x) : (x)) extern "C" void shutdownCallback(void *pPvt) @@ -56,9 +59,10 @@ extern "C" void shutdownCallback(void *pPvt) } // These are the pmacAxis class methods -pmacAxis::pmacAxis(pmacController *pC, int axisNo) +pmacAxis::pmacAxis(pmacController *pC, int axisNo, bool enable) : SINQAxis(pC, axisNo), - pC_(pC) + pC_(pC), + autoEnable(enable) { static const char *functionName = "pmacAxis::pmacAxis"; @@ -80,6 +84,7 @@ pmacAxis::pmacAxis(pmacController *pC, int axisNo) status6Time = 0; starting = 0; homing = 0; + next_poll = -1; /* Set an EPICS exit handler that will shut down polling before asyn kills the IP sockets */ epicsAtExit(shutdownCallback, pC_); @@ -156,13 +161,18 @@ asynStatus pmacAxis::getAxisInitialStatus(void) } // Enable the axis. After startup, the axis are disabled on the controller... - sprintf(command, "M%2.2d14=1", axisNo_); - cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); - if (cmdStatus ) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: enaabling axis %d failed.\n", functionName, axisNo_); - return asynError; - } + // Warning: Selene lift axis should not be automatically enabled + if (autoEnable) { + sprintf(command, "M%2.2d14=1\n", axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Enable axis %d: %s",axisNo_,command); + cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + if (cmdStatus ) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: enaabling axis %d failed.\n", functionName, axisNo_); + return asynError; + } + } + callParamCallbacks(); return asynSuccess; @@ -202,7 +212,9 @@ asynStatus pmacAxis::move(double position, int relative, double min_velocity, do sprintf( command, "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", axisNo_, axisNo_, realPosition, axisNo_); status = pC_->lowLevelWriteRead(axisNo_,command, response); - + + next_poll = -1; + return status; } @@ -224,6 +236,8 @@ asynStatus pmacAxis::home(double min_velocity, double max_velocity, double accel status = pC_->lowLevelWriteRead(axisNo_,command, response); homing = 1; + next_poll = time(NULL) + BUSYPOLL; + return status; } @@ -277,6 +291,11 @@ asynStatus pmacAxis::poll(bool *moving) static const char *functionName = "pmacAxis::poll"; char message[132]; + // Protect against excessive polling + if(time(NULL) < next_poll){ + return asynSuccess; + } + sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_); pC_->debugFlow(message); @@ -285,7 +304,13 @@ asynStatus pmacAxis::poll(bool *moving) asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: getAxisStatus failed to return asynSuccess. Controller: %s, Axis: %d.\n", functionName, pC_->portName, axisNo_); } - + + if(*moving){ + next_poll = time(NULL) + BUSYPOLL; + } else { + next_poll = time(NULL) + IDLEPOLL; + } + callParamCallbacks(); return status ? asynError : asynSuccess; } @@ -346,8 +371,8 @@ asynStatus pmacAxis::getAxisStatus(bool *moving) int done = 0, posChanging = 0; double position = 0; int nvals = 0; - int axisProblemFlag = 0; - epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim= 0; + int axisProblemFlag = 0, axStat = 0; + epicsUInt32 axErr = 0, highLim = 0, lowLim= 0; char message[132], *axMessage; @@ -420,7 +445,7 @@ asynStatus pmacAxis::getAxisStatus(bool *moving) previous_position_ = position; previous_direction_ = direction; - // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, axErr, position); + errlogPrintf("Polling %d, axStat = %d, axErr = %d, position = %f\n", axisNo_, axStat, axErr, position); /* are we done? */ if((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0 ){ @@ -618,7 +643,9 @@ asynStatus SeleneAxis::move(double position, int relative, double min_velocity, errlogPrintf("Sending drive command: %s\n", command); status = pC_->lowLevelWriteRead(axisNo_,command, response); - + + next_poll = -1; + return status; } /*----------------------------------------------------------------------------------------------------*/ @@ -656,6 +683,8 @@ asynStatus LiftAxis::move(double position, int relative, double min_velocity, status = pC_->lowLevelWriteRead(axisNo_,command, response); waitStart = 1; + next_poll = -1; + return status; } /*-------------------------------------------------------------------------------------------------------- @@ -666,6 +695,11 @@ asynStatus LiftAxis::poll(bool *moving) { asynStatus status; + // Protect against excessive polling + if(time(NULL) < next_poll){ + return asynSuccess; + } + status = getAxisStatus(moving); if(*moving == false && waitStart == 1){ *moving = true; @@ -675,6 +709,12 @@ asynStatus LiftAxis::poll(bool *moving) if(*moving){ waitStart = 0; } + + if(*moving){ + next_poll = time(NULL) + BUSYPOLL; + } else { + next_poll = time(NULL) + IDLEPOLL; + } callParamCallbacks(); return status; } diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 824f91d..d7a297b 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -28,7 +28,7 @@ class pmacAxis : public SINQAxis { public: /* These are the methods we override from the base class */ - pmacAxis(pmacController *pController, int axisNo); + pmacAxis(pmacController *pController, int axisNo, bool enable=true); virtual ~pmacAxis(); asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); @@ -62,6 +62,10 @@ class pmacAxis : public SINQAxis int homing; double statusPos; + time_t next_poll; + + bool autoEnable; + friend class pmacController; }; @@ -84,7 +88,7 @@ class SeleneAxis : public pmacAxis { public: - SeleneAxis(SeleneController *pController, int axisNo, double limitTarget) : pmacAxis((pmacController *)pController,axisNo) + SeleneAxis(SeleneController *pController, int axisNo, double limitTarget) : pmacAxis((pmacController *)pController, axisNo, false) { this->limitTarget = limitTarget; }; @@ -113,11 +117,15 @@ class SeleneAxis : public pmacAxis Mark Koennecke, February 2020 + The axis should not be enabled automatically + + Michele Brambilla, February 2020 + */ class LiftAxis : public pmacAxis { public: - LiftAxis(pmacController *pController, int axisNo) : pmacAxis((pmacController *)pController,axisNo) {}; + LiftAxis(pmacController *pController, int axisNo) : pmacAxis((pmacController *)pController,axisNo,false) {}; asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); asynStatus poll(bool *moving); asynStatus stop(double acceleration); diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index a4975ed..2884b77 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -726,14 +726,14 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu // 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/MULT)); + sprintf(command, "Q%d54=%f", pAxis->axisNo_, value/MULT); // 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/MULT)); + sprintf(command, "Q%d53=%f", pAxis->axisNo_, value/MULT); // 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", diff --git a/testEuroMoveUsb.py b/testEuroMoveUsb.py new file mode 100755 index 0000000..8fd6989 --- /dev/null +++ b/testEuroMoveUsb.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +# -*- coding: iso-8859-1 -*- +""" +Created on Thur Feb 06 10:22:42 2020 + +@author: gaston emmanuel exil +""" + +#pip install pyusb +#sudo cp 10-llbDevices.rules /etc/udev/rules.d/ +#sudo udevadm control --reload-rules && udevadm trigger +#reboot + +import sys +import usb.core +import usb.util +from time import sleep + +# 1. Device +llbVendorID=0x04B4 +llbProductID=0x1002 + +# 2. Configuration +CONFIGURATION_EV3 = 1 # 1-based +# 3. Interface +INTERFACE_EV3 = 0 # 0-based +# 4. Alternate setting +SETTING_EV3 = 0 # 0-based +# 5. Endpoint +ENDPOINT_EV3 = 1 # 0-based + +# find our device +device = usb.core.find(idVendor=llbVendorID, idProduct=llbProductID) + +# was it found? +if device is None: + raise ValueError('Device not found') + sys.exit(1) +else: + if device._manufacturer is None: + device._manufacturer = usb.util.get_string(device, device.iManufacturer) + print("manufacturer: ", str(device._manufacturer)) + if device._product is None: + device._product = usb.util.get_string(device, device.iProduct) + print("product: ", str(device._product)) + + +# By default, the kernel will claim the device and make it available via +# /dev/usb/hiddevN and /dev/hidrawN which also prevents us +# from communicating otherwise. This removes these kernel devices. +# Yes, it is weird to specify an interface before we get to a configuration. + +if device.is_kernel_driver_active(INTERFACE_EV3): + print("Detaching kernel driver") + device.detach_kernel_driver(INTERFACE_EV3) + + # claim the device + usb.util.claim_interface(device, INTERFACE_EV3) + +# write endpoint +endpoint_BulkOut = device[0][(0,0)][0] +# Read endpoint +endpoint_BulkIn = device[0][(0,0)][2] +try: + command = "L"+chr(13) + assert device.write(endpoint_BulkOut, command.encode('utf-8'), 100) == len(command) + ret = device.read(endpoint_BulkIn, endpoint_BulkIn.wMaxPacketSize, timeout=30000) + print(ret) + #byte_str = "".join(chr(n) for n in ret) + print(ret.tostring()) + + + + command = "A1,4"+chr(13) + assert device.write(endpoint_BulkOut, command.encode('utf-8'), 100) == len(command) + ret = device.read(endpoint_BulkIn, endpoint_BulkIn.wMaxPacketSize, timeout=30000) + print(ret) + byte_str = "".join(chr(n) for n in ret) + print(ret.tostring()) + +except Exception as e: + print(e) + + +# release the device +usb.util.release_interface(device, INTERFACE_EV3) +# reattach the device to the OS kernel +#device.attach_kernel_driver(INTERFACE_EV3)