From e657ea675ce6bcbd8159e6a9f3e182013441475b Mon Sep 17 00:00:00 2001 From: wall_e Date: Tue, 10 Sep 2024 09:21:15 +0200 Subject: [PATCH] SINQSW-86 fix build error --- .gitignore | 1 + Makefile.RHEL8 | 2 +- sinqEPICSApp/Db/sinq_asyn_motor.db | 15 ++ sinqEPICSApp/src/pmacAxis.cpp | 10 +- sinqEPICSApp/src/pmacController.cpp | 225 +++++++++++++++++++--------- sinqEPICSApp/src/pmacController.h | 6 +- sinqEPICSApp/src/sinq.dbd | 1 - utils/deltatau.py | 140 ++++++++++++++++- 8 files changed, 317 insertions(+), 83 deletions(-) diff --git a/.gitignore b/.gitignore index b205e48..f7668fd 100644 --- a/.gitignore +++ b/.gitignore @@ -66,6 +66,7 @@ local.properties *.user *.sln.docstates *.sdf +.cvsignore #Test results *.log diff --git a/Makefile.RHEL8 b/Makefile.RHEL8 index 0d24e3d..eabbf77 100644 --- a/Makefile.RHEL8 +++ b/Makefile.RHEL8 @@ -15,7 +15,7 @@ REQUIRED+=asynMotor # using a test version #scaler_VERSION=2024 -LIBVERSION=2024-dev +LIBVERSION=2024-v2 TEMPLATES += sinqEPICSApp/Db/dimetix.db TEMPLATES += sinqEPICSApp/Db/slsvme.db diff --git a/sinqEPICSApp/Db/sinq_asyn_motor.db b/sinqEPICSApp/Db/sinq_asyn_motor.db index 36bdbb6..61e2878 100644 --- a/sinqEPICSApp/Db/sinq_asyn_motor.db +++ b/sinqEPICSApp/Db/sinq_asyn_motor.db @@ -46,3 +46,18 @@ record(longin, "$(P)$(M):Enable_RBV") { field(SCAN, "1 second") } +# reread encoder +record(longout, "$(P)$(M):Reread_Encoder") { + field(DTYP, "asynInt32") + field(OUT, "@asyn($(PORT),$(N),1) REREAD_ENCODER_POSITION") + field(PINI, "YES") +} + +# reread encoder +record(longin, "$(P)$(M):Reread_Encoder_RBV") { + field(DTYP, "asynInt32") + field(INP, "@asyn($(PORT),$(N),1) REREAD_ENCODER_POSITION_RBV") + field(PINI, "NO") + field(SCAN, "1 second") +} + diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index 308673a..4c9de42 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -925,6 +925,7 @@ asynStatus pmacV3Axis::poll(bool *moving) { } callParamCallbacks(); + return status ? asynError : asynSuccess; } @@ -950,10 +951,11 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { if (cmdStatus || nvals != 3) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "drvPmacAxisGetStatus: Failed to read position and status, " - "Status: %d\nCommand :%s\nResponse:%s\n", - cmdStatus, command, response); + "Status: %d\nCommand :%s\nParsed: %d\nResponse:%s\n", + cmdStatus, command, nvals, response); updateMsgTxtFromDriver("Cannot read Axis position and status"); } + pmacV3Controller *p3C_ = (pmacV3Controller *)pC_; IsEnable = axStat != -3; @@ -1063,6 +1065,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { cmdStatus = cmdStatus > st ? cmdStatus : st; nvals = sscanf(response, "%d %d %d", &axErr, &highLim, &lowLim); + if (st || nvals != 3) { asynPrint( pC_->pasynUserSelf, ASYN_TRACE_ERROR, @@ -1073,9 +1076,6 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) { "Cannot read Axis Error, high and low limit flag Status"); } - // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, - // axErr, position); - sprintf(message, "poll results: axis %d, status %d, axErr %d, done = %d", axisNo_, axStat, axErr, done); pC_->debugFlow(message); diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index 71c7e2f..13dd2b2 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -498,24 +498,20 @@ return pAxes_[axisNo]; /** Polls the controller, rather than individual axis.*/ asynStatus pmacController::poll() { -static const char *functionName = "pmacController::poll"; + static const char *functionName = "pmacController::poll"; -debugFlow(functionName); + debugFlow(functionName); -if (!lowLevelPortUser_) { -setIntegerParam(this->motorStatusCommsError_, 1); -return asynError; + if (!lowLevelPortUser_) { + setIntegerParam(this->motorStatusCommsError_, 1); + return asynError; + } + + asynMotorController::poll(); + + return callParamCallbacks(); } -asynMotorController::poll(); - -callParamCallbacks(); - -return asynSuccess; -} - - - /*************************************************************************************/ /** The following functions have C linkage, and can be called directly or from iocsh */ @@ -571,17 +567,22 @@ createParam(MotorSetPositionString, asynParamFloat64, &se callParamCallbacks(); } -pmacV3Controller::pmacV3Controller(const char *portName, - const char *lowLevelPortName, - int lowLevelPortAddress, int numAxes, - double movingPollPeriod, - double idlePollPeriod, - const int &extraParams) : -pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) { -static const char *functionName = "pmacV3Controller::pmacV3Controller"; -createParam(EnableAxisString, asynParamInt32, &enableAxis_); -createParam(AxisEnabledString, asynParamInt32, &axisEnabled_); -callParamCallbacks(); +pmacV3Controller::pmacV3Controller( + const char *portName, + const char *lowLevelPortName, + int lowLevelPortAddress, int numAxes, + double movingPollPeriod, + double idlePollPeriod, + const int &extraParams +) : pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) { + + static const char *functionName = "pmacV3Controller::pmacV3Controller"; + createParam(EnableAxisString, asynParamInt32, &enableAxis_); + createParam(AxisEnabledString, asynParamInt32, &axisEnabled_); + createParam(ReReadEncoderPositionString, asynParamInt32, &rereadEncoderPosition_); + createParam(ReReadEncoderPositionString_RBV, asynParamInt32, &rereadEncoderPositionRBV_); + callParamCallbacks(); + } /** @@ -871,19 +872,20 @@ return status; asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { -int function = pasynUser->reason; -asynStatus status = asynSuccess; -pmacV3Axis *pAxis = NULL; -char command[64] = {0}; -char response[64] = {0}; -int isOn; -time_t startTime; -bool moving; -static const char *functionName = "pmacV3Controller::writeInt32"; + + int function = pasynUser->reason; + asynStatus status = asynSuccess; + pmacV3Axis *pAxis = NULL; + char command[64] = {0}; + char response[64] = {0}; + int isOn; + time_t startTime; + bool moving; + static const char *functionName = "pmacV3Controller::writeInt32"; debugFlow(functionName); - pAxis = (pmacV3Axis*)this->getAxis(pasynUser); + pAxis = (pmacV3Axis*) this->getAxis(pasynUser); if (!pAxis) { return asynError; } @@ -893,43 +895,125 @@ static const char *functionName = "pmacV3Controller::writeInt32"; pAxis->setIntegerParam(function, value); if (function == enableAxis_) { - // only send commands when a state change is necessary + + // Get Current Status snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_); this->lowLevelWriteRead(pAxis->axisNo_, command, response); isOn = strtol(response, NULL, 10) != -3; - /* This was an attempt at automatically reading the encoder. - This is disabled as it did not work. - */ - if(value == 1 && isOn) { - sprintf(command, "M%2.2d=15", pAxis->axisNo_); - // lowLevelWriteRead(pAxis->axisNo_, command, response); - } - // Valid code continues here - sprintf(command, "M%2.2d14=%d", pAxis->axisNo_, value); - pAxis->updateMsgTxtFromDriver(""); - if(isOn != value) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, - "%s: Enable axis on controller %s, axis %d enable=%d\n", - functionName, portName, pAxis->axisNo_, value); - lowLevelWriteRead(pAxis->axisNo_, command, response); - startTime = time(NULL); - while(time(NULL) < startTime + 2.){ - snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_); - this->lowLevelWriteRead(pAxis->axisNo_, command, response); - isOn = strtol(response, NULL, 10) != -3; - if(value == isOn){ - pAxis->poll(&moving); // to update enable status - return asynSuccess; - } - usleep(100); - } - errlogPrintf("PMAC: failed to switch axis %d enable status within 2 seconds\n", - pAxis->axisNo_); - return asynError; - } - } + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d on controller %s %s on\n", + functionName, pAxis->axisNo_, portName, isOn ? "is" : "isn't"); + usleep(100); - return pmacController::writeInt32(pasynUser, value); + if (isOn == value) { + return asynSuccess; + } + + // Switch On/Off + snprintf(command, sizeof(command), "M%2.2d14=%d", pAxis->axisNo_, value); + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: %s axis %d on controller %s\n", + functionName, value ? "Enable" : "Disable", pAxis->axisNo_, portName); + this->lowLevelWriteRead(pAxis->axisNo_, command, response); + + // Wait for motor status to change + snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_); + startTime = time(NULL); + while (time(NULL) < startTime + 2.) { + usleep(100); + this->lowLevelWriteRead(pAxis->axisNo_, command, response); + isOn = strtol(response, NULL, 10) != -3; + if(isOn == value) { + pAxis->poll(&moving); // to update enable status + return asynSuccess; + } + } + + errlogPrintf( + "PMAC: Failed to %s axis %d on controller %s within 2 seconds\n", + value ? "enable" : "disable", pAxis->axisNo_, portName); + return asynError; + + } else if (function == rereadEncoderPosition_) { + // This is not a command that can always be run when enabling a motor as it + // also causes relative encoders to reread a position necessitating + // recalibration. We only want it to run on absolute encoders. We also want + // it to be clear to instrument scientists, that power has to be cut to the + // motor, in order to reread the encoder as not all motors have breaks and + // they may start to move when disabled. For that reason, we don't + // automatically disable the motors to run the command and instead require + // that the scientists first disable the motor. + + if (!value) { + return asynSuccess; + } + + // Check if this is an aboslute encoder + snprintf(command, sizeof(command), "I%2.2X04", pAxis->axisNo_); + this->lowLevelWriteRead(pAxis->axisNo_, command, response); + + int reponse_length = strlen(response); + if (reponse_length < 3) { + errlogPrintf( + "%s: Unexpected reponse '%s' from axis %d on controller %s while rereading encoder. Aborting...\n", + functionName, response, pAxis->axisNo_, portName); + return asynError; + } + + // We are only interested in the last two digits and the last value in the + // string before the terminator is \r + int encoder_id = 0; + sscanf(response + (reponse_length - 3), "%2X", &encoder_id); + + snprintf(command, sizeof(command), "P46", pAxis->axisNo_); + this->lowLevelWriteRead(pAxis->axisNo_, command, response); + int number_of_axes = strtol(response, NULL, 10); + + if (encoder_id <= number_of_axes) { + errlogPrintf( + "%s: Trying to reread absolute encoder of axis %d on controller %s, but it is a relative encoder. Aborting...\n", + functionName, pAxis->axisNo_, portName); + return asynError; + } + + // Get Current Status + snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_); + this->lowLevelWriteRead(pAxis->axisNo_, command, response); + isOn = strtol(response, NULL, 10) != -3; + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Axis %d on controller %s %s on\n", + functionName, pAxis->axisNo_, portName, isOn ? "is" : "isn't"); + + if (isOn) { + errlogPrintf( + "%s: First disable axis %d on controller %s to reread encoder position\n", + functionName, pAxis->axisNo_, portName); + return asynError; + } + + // Reread current absolute position if possible + sprintf(command, "M%2.2d=15", pAxis->axisNo_); + asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, + "%s: Rereading Absolute Encoder of axis %d on controller %s via command %s\n", + functionName, pAxis->axisNo_, portName, command); + this->lowLevelWriteRead(pAxis->axisNo_, command, response); + + // Switching on the axis while the rereading process is still ongoing + // causes it to fail. We currently have no way to check if it is actually + // finished, so we instead wait for 0.5 seconds. + usleep(500000); + + // turn off parameter as finished rereading + // this will only be immediately noticed in the read back variable though + pAxis->setIntegerParam(function, 0); + + return asynSuccess; + + } else { + + return pmacController::writeInt32(pasynUser, value); + + } } asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) { @@ -954,7 +1038,12 @@ asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) { axStat = *value != -3; int st = setIntegerParam(axisEnabled_, axStat); callParamCallbacks(); + } else if (function == rereadEncoderPositionRBV_) { + getIntegerParam(pAxis->axisNo_, rereadEncoderPosition_, value); + setIntegerParam(pAxis->axisNo_, rereadEncoderPositionRBV_, *value); + return callParamCallbacks(); } + return pmacController::readInt32(pasynUser, value); } diff --git a/sinqEPICSApp/src/pmacController.h b/sinqEPICSApp/src/pmacController.h index b87d846..ac84749 100644 --- a/sinqEPICSApp/src/pmacController.h +++ b/sinqEPICSApp/src/pmacController.h @@ -168,12 +168,14 @@ class SeleneController : public pmacController { #define EnableAxisString "ENABLE_AXIS" #define AxisEnabledString "AXIS_ENABLED" +#define ReReadEncoderPositionString "REREAD_ENCODER_POSITION" +#define ReReadEncoderPositionString_RBV "REREAD_ENCODER_POSITION_RBV" class pmacV3Controller : public pmacController { public: pmacV3Controller(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, int numAxes, double movingPollPeriod, - double idlePollPeriod, const int &extraParams = 2); + double idlePollPeriod, const int &extraParams = 4); // overloaded because we want to enable/disable the motor asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); @@ -190,6 +192,8 @@ protected: int enableAxis_; int axisEnabled_; + int rereadEncoderPosition_; + int rereadEncoderPositionRBV_; }; #endif /* pmacController_H */ diff --git a/sinqEPICSApp/src/sinq.dbd b/sinqEPICSApp/src/sinq.dbd index f605942..3957538 100644 --- a/sinqEPICSApp/src/sinq.dbd +++ b/sinqEPICSApp/src/sinq.dbd @@ -7,7 +7,6 @@ registrar(EuroMoveRegister) registrar(NanotecRegister) registrar(pmacControllerRegister) registrar(pmacAsynIPPortRegister) -registrar(drvAsynMMACSPortRegisterCommands) registrar(MasterMACSRegister) registrar(SINQControllerRegister) diff --git a/utils/deltatau.py b/utils/deltatau.py index fda4bad..c4aaeb7 100755 --- a/utils/deltatau.py +++ b/utils/deltatau.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 """ This is some code for communicating with a Delta-Tau PMAC motor controller as used at SINQ from python. Python 3 is assumed and @@ -14,6 +15,7 @@ import struct import socket +import curses def packPmacCommand(command): # 0x40 = VR_DOWNLOAD @@ -41,11 +43,135 @@ def readPmacReply(input): if __name__ == "__main__": from sys import argv - addr = argv[1].split(':') - s = socket.socket(socket.AF_INET,socket.SOCK_STREAM) - s.connect((addr[0],int(addr[1]))) - buf = packPmacCommand(argv[2]) - s.send(buf) - reply = readPmacReply(s) - print(reply.decode('utf-8') + '\n') + try: + + addr = argv[1].split(':') + s = socket.socket(socket.AF_INET,socket.SOCK_STREAM) + s.connect((addr[0],int(addr[1]))) + + if len(argv) == 3: + buf = packPmacCommand(argv[2]) + s.send(buf) + reply = readPmacReply(s) + print(reply.decode('utf-8') + '\n') + + else: + + try: + + stdscr = curses.initscr() + curses.noecho() + curses.cbreak() + stdscr.keypad(True) + + stdscr.addstr(">> ") + stdscr.refresh() + + history = [""] + ptr = len(history) - 1 + + while True: + c = stdscr.getch() + if c == curses.KEY_RIGHT: + (y, x) = stdscr.getyx() + if x < len(history[ptr]) + 3: + stdscr.move(y, x+1) + stdscr.refresh() + elif c == curses.KEY_LEFT: + (y, x) = stdscr.getyx() + if x > 3: + stdscr.move(y, x-1) + stdscr.refresh() + elif c == curses.KEY_UP: + if ptr > 0: + ptr -= 1 + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + elif c == curses.KEY_DOWN: + if ptr < len(history) - 1: + ptr += 1 + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + elif c == curses.KEY_ENTER or c == ord('\n') or c == ord('\r'): + if history[ptr] == 'quit': + break + + # because of arrow keys move back to the end of the line + (y, x) = stdscr.getyx() + stdscr.move(y, 3+len(history[ptr])) + + if history[ptr]: + buf = packPmacCommand(history[ptr]) + s.send(buf) + reply = readPmacReply(s) + stdscr.addstr("\n" + reply.decode('utf-8')[0:-1]) + + if ptr == len(history) - 1 and history[ptr] != "": + history += [""] + else: + history[-1] = "" + ptr = len(history) - 1 + + stdscr.addstr("\n>> ") + stdscr.refresh() + + else: + if ptr < len(history) - 1: # Modifying previous input + if len(history[-1]) == 0: + history[-1] = history[ptr] + ptr = len(history) - 1 + + else: + history += [history[ptr]] + ptr = len(history) - 1 + + if c == curses.KEY_BACKSPACE: + if len(history[ptr]) == 0: + continue + (y, x) = stdscr.getyx() + history[ptr] = history[ptr][0:x-4] + history[ptr][x-3:] + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + stdscr.move(y, x-1) + stdscr.refresh() + + else: + (y, x) = stdscr.getyx() + history[ptr] = history[ptr][0:x-3] + chr(c) + history[ptr][x-3:] + stdscr.addch("\r") + stdscr.clrtoeol() + stdscr.addstr(">> " + history[ptr]) + stdscr.move(y, x+1) + stdscr.refresh() + + finally: + + # to quit + curses.nocbreak() + stdscr.keypad(False) + curses.echo() + curses.endwin() + + except: + print(""" + Invalid Arguments + + Option 1: Single Command + ------------------------ + + Usage: deltatau.py pmachost:port command + This then returns the response for command. + + Option 2: CLI Mode + ------------------ + + Usage: deltatau.py pmachost:port + + You can then type in a command, hit enter, and the response will see + the reponse, before being prompted to again enter a command. Type + 'quit' to close prompt. + """)