From bf2ff63a4b398fe5fe13e9fbd7bfc90bbc399991 Mon Sep 17 00:00:00 2001 From: koennecke Date: Thu, 23 May 2019 16:50:10 +0200 Subject: [PATCH] Fixed bugs in EL734 Phytron motor driver is now working May be fixed inverted limits on pmac --- .../iocsinqEPICS/motor.substitutions.phytron | 10 +-- iocBoot/iocsinqEPICS/phytest.cmd | 25 +++++++ iocBoot/iocsinqEPICS/phytrontest.cmd | 5 +- sinqEPICSApp/Db/asynRecord.db | 9 +++ sinqEPICSApp/Db/basic_asyn_motor.db | 23 ++++++ sinqEPICSApp/Db/basic_motor.db | 21 ++++++ sinqEPICSApp/src/EL734Driver.cpp | 70 +++++++++++-------- sinqEPICSApp/src/EL734Driver.h | 4 +- sinqEPICSApp/src/PhytronDriver.cpp | 63 ++++++++++++----- sinqEPICSApp/src/pmacAxis.cpp | 16 ++++- 10 files changed, 183 insertions(+), 63 deletions(-) create mode 100755 iocBoot/iocsinqEPICS/phytest.cmd create mode 100644 sinqEPICSApp/Db/asynRecord.db create mode 100644 sinqEPICSApp/Db/basic_asyn_motor.db create mode 100644 sinqEPICSApp/Db/basic_motor.db diff --git a/iocBoot/iocsinqEPICS/motor.substitutions.phytron b/iocBoot/iocsinqEPICS/motor.substitutions.phytron index a94780b..4547179 100644 --- a/iocBoot/iocsinqEPICS/motor.substitutions.phytron +++ b/iocBoot/iocsinqEPICS/motor.substitutions.phytron @@ -1,12 +1,12 @@ -file "$(MOTOR)/db/basic_asyn_motor.db" +file "$(BASE)/sinqEPICSApp/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} -{KM36:phytron:, 1, "m$(N)", "asynMotor", phy, 1, "m1", mm, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 100, -100, "1"} -{KM36:phytron:, 2, "m$(N)", "asynMotor", phy, 2, "m2", mm, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 100, -100, "10"} +{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, RDBD, DHLM, DLLM, INIT} +{KM36:phytron:, 1, "m$(N)", "asynMotor", phy, 1, "m1", mm, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 0.2, 100, -100, "1"} +{KM36:phytron:, 2, "m$(N)", "asynMotor", phy, 2, "m2", mm, Pos, 2.0, 0.1, .2, 0, 1, .2, .001, 3, 0.2, 100, -100, "10"} } -file "$(SINQ)/Db/motorMessage.db" +file "$(BASE)/sinqEPICSApp/Db/motorMessage.db" { pattern {P,N, M,PORT} diff --git a/iocBoot/iocsinqEPICS/phytest.cmd b/iocBoot/iocsinqEPICS/phytest.cmd new file mode 100755 index 0000000..84a7076 --- /dev/null +++ b/iocBoot/iocsinqEPICS/phytest.cmd @@ -0,0 +1,25 @@ +#!/usr/local/bin/iocsh + +require sinq,koennecke + +epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/iocBoot/iocsinqEPICS") +epicsEnvSet("BASE","/afs/psi.ch/project/sinqdev/sinqepicsapp") +epicsEnvSet("dbPATH","${EPICS_BASE}/dbd:${ASYN}/dbd:${MOTOR}/dbd") + +cd ${TOP} + +## Register all support components +dbLoadDatabase "../../dbd/sinqEPICS.dbd" + + +#drvAsynIPPortConfigure("serial1", "129.129.195.58:22222",0,0,0) +drvAsynIPPortConfigure("serial1", "localhost:9090",0,0,0) +PhytronCreateController("phy","serial1","0",1,0); + +### Motors + +dbLoadRecords("$(BASE)/sinqEPICSApp/Db/asynRecord.db","P=KM36:,R=serial1,PORT=serial1,ADDR=0,OMAX=80,IMAX=80") +dbLoadTemplate "motor.substitutions.phytron" + + +iocInit diff --git a/iocBoot/iocsinqEPICS/phytrontest.cmd b/iocBoot/iocsinqEPICS/phytrontest.cmd index db9ebb3..7a0939a 100755 --- a/iocBoot/iocsinqEPICS/phytrontest.cmd +++ b/iocBoot/iocsinqEPICS/phytrontest.cmd @@ -18,12 +18,9 @@ epicsEnvSet("dbPATH","${EPICS_BASE}/dbd::${ASYN}/dbd:${MOTOR}/dbd:${ANC}/dbd") cd ${TOP} ## Register all support components -dbLoadDatabase "../../dbd/sinqEPICS.dbd" -#dbLoadDatabase "dbd/sinq.dbd" +dbLoadDatabase "../../sinqEPICSApp/src/sinq.dbd" sinqEPICS_registerRecordDeviceDriver pdbbase -## Load record instances -#dbLoadRecords("db/xxx.db","user=koenneckeHost") #---------- load Nanotec motor controller diff --git a/sinqEPICSApp/Db/asynRecord.db b/sinqEPICSApp/Db/asynRecord.db new file mode 100644 index 0000000..6c50e6d --- /dev/null +++ b/sinqEPICSApp/Db/asynRecord.db @@ -0,0 +1,9 @@ +record(asyn,"$(P)$(R)") +{ + field(DTYP,"asynRecordDevice") + field(PORT,"$(PORT)") + field(ADDR,"$(ADDR)") + field(OMAX,"$(OMAX)") + field(IMAX,"$(IMAX)") +} + diff --git a/sinqEPICSApp/Db/basic_asyn_motor.db b/sinqEPICSApp/Db/basic_asyn_motor.db new file mode 100644 index 0000000..fc19187 --- /dev/null +++ b/sinqEPICSApp/Db/basic_asyn_motor.db @@ -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") +} + diff --git a/sinqEPICSApp/Db/basic_motor.db b/sinqEPICSApp/Db/basic_motor.db new file mode 100644 index 0000000..98e55ec --- /dev/null +++ b/sinqEPICSApp/Db/basic_motor.db @@ -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") +} + diff --git a/sinqEPICSApp/src/EL734Driver.cpp b/sinqEPICSApp/src/EL734Driver.cpp index 2a417a6..ca6fda1 100644 --- a/sinqEPICSApp/src/EL734Driver.cpp +++ b/sinqEPICSApp/src/EL734Driver.cpp @@ -272,6 +272,7 @@ asynStatus EL734Axis::move(double position, int relative, double minVelocity, do } oredMSR = 0; homing = 0; + errorReported = 0; //errlogPrintf("Starting axis %d with destination %f", axisNo_,position/1000); sprintf(command, "p %d %.3f", axisNo_, position/1000.); status = pC_->transactController(axisNo_,command,reply); @@ -291,6 +292,7 @@ asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double accele setIntegerParam(pC_->motorStatusProblem_, false); updateMsgTxtFromDriver(""); + errorReported = 0; sprintf(command, "R %d", axisNo_); homing = 1; @@ -310,6 +312,7 @@ asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, doubl // functionName, minVelocity, maxVelocity, acceleration); + errorReported = 0; if (maxVelocity > 0.) { /* This is a positive move */ sprintf(command, "FF %d", axisNo_); @@ -326,14 +329,16 @@ asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, doubl asynStatus EL734Axis::stop(double acceleration ) { - asynStatus status; + asynStatus status = asynSuccess; //static const char *functionName = "EL734Axis::stop"; char command[COMLEN], reply[COMLEN]; - sprintf(command, "S %d", axisNo_); - status = pC_->transactController(axisNo_,command,reply); - errlogPrintf("Sent STOP on Axis %d\n", axisNo_); - updateMsgTxtFromDriver("Axis interrupted"); + if(errorReported == 0){ + sprintf(command, "S %d", axisNo_); + status = pC_->transactController(axisNo_,command,reply); + errlogPrintf("Sent STOP on Axis %d\n", axisNo_); + updateMsgTxtFromDriver("Axis interrupted"); + } return status; } @@ -372,10 +377,8 @@ asynStatus EL734Axis::setClosedLoop(bool closedLoop) asynStatus EL734Axis::poll(bool *moving) { int msr, count; - asynStatus comStatus; + asynStatus comStatus = asynSuccess; char command[COMLEN], reply[COMLEN], errTxt[256]; - int driverError = 0; - // protect against excessive polling if(time(NULL) < next_poll){ @@ -384,31 +387,33 @@ asynStatus EL734Axis::poll(bool *moving) } // Read the current motor position + setIntegerParam(pC_->motorStatusProblem_,false); sprintf(command,"u %d", axisNo_); comStatus = pC_->transactController(axisNo_,command,reply); if(comStatus == asynError){ - driverError = 1; + setIntegerParam(pC_->motorStatusProblem_,true); + goto skip; } - if(comStatus) goto skip; if(strstr(reply,"*ES") != NULL){ *moving = false; setIntegerParam(pC_->motorStatusDone_, true); setIntegerParam(pC_->motorStatusProblem_, true); + errorReported = 1; updateMsgTxtFromDriver("Emergency Stop Engaged"); - driverError = 1; + comStatus = asynError; goto skip; } else if(strstr(reply,"?BSY") != NULL){ *moving = true; setIntegerParam(pC_->motorStatusDone_, false); - updateMsgTxtFromDriver(""); goto skip; } count = sscanf(reply,"%lf", &position); if(count != 1) { snprintf(errTxt,sizeof(errTxt),"Bad reply %s when reading position for %d", reply, axisNo_); + setIntegerParam(pC_->motorStatusProblem_, true); + errorReported = 1; updateMsgTxtFromDriver(errTxt); comStatus = asynError; - driverError =1; goto skip; } //errlogPrintf("Axis %d, reply %s, position %lf\n", axisNo_, reply, position); @@ -419,7 +424,10 @@ asynStatus EL734Axis::poll(bool *moving) // Read the moving status of this motor sprintf(command,"msr %d",axisNo_); comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; + if(comStatus == asynError){ + setIntegerParam(pC_->motorStatusProblem_,true); + goto skip; + } sscanf(reply,"%x",&msr); //errlogPrintf("Axis %d, reply %s, msr %d, oredmsr = %d, position = %lf\n", @@ -429,54 +437,58 @@ asynStatus EL734Axis::poll(bool *moving) if( (msr & 0x1) == 0){ // done: check for trouble //errlogPrintf("Axis %d finished\n", axisNo_); + *moving = false; + setIntegerParam(pC_->motorStatusDone_, true); + next_poll = time(NULL)+IDLEPOLL; if(oredMSR & 0x10){ setIntegerParam(pC_->motorStatusLowLimit_, true); updateMsgTxtFromDriver("Lower Limit Hit"); - driverError = 1; + errorReported = 1; + comStatus = asynError; + goto skip; } else { setIntegerParam(pC_->motorStatusLowLimit_, false); } if(oredMSR & 0x20){ setIntegerParam(pC_->motorStatusHighLimit_, true); updateMsgTxtFromDriver("Upper Limit Hit"); - driverError = 1; + errorReported = 1; + comStatus = asynError; + goto skip; } else { setIntegerParam(pC_->motorStatusHighLimit_, false); } if(homing){ setIntegerParam(pC_->motorStatusAtHome_, true); } - setIntegerParam(pC_->motorStatusProblem_, false); if(oredMSR & 0x1000){ setIntegerParam(pC_->motorStatusProblem_, true); // errlogPrintf("Detected air cushion error on %d", axisNo_); errlogSevPrintf(errlogMajor, "Air cushion problem on %d", axisNo_); updateMsgTxtFromDriver("Air cushion error"); - driverError = 1; + errorReported = 1; + comStatus = asynError; + goto skip; } if(oredMSR &0x80){ setIntegerParam(pC_->motorStatusProblem_, true); errlogSevPrintf(errlogMajor, "Positioning fault at %d", axisNo_); updateMsgTxtFromDriver("Positioning fault"); - driverError = 1; + comStatus = asynError; + errorReported = 1; + goto skip; } - *moving = false; - setIntegerParam(pC_->motorStatusDone_, true); - } else { + setIntegerParam(pC_->motorStatusProblem_, false); +} else { *moving = true; next_poll = -1; setIntegerParam(pC_->motorStatusDone_, false); - updateMsgTxtFromDriver("Creeping"); - } +} skip: - if(driverError == 0){ - updateMsgTxtFromDriver(""); - } - setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0); callParamCallbacks(); - return comStatus ? asynError : asynSuccess; + return comStatus; } /** Code for iocsh registration */ diff --git a/sinqEPICSApp/src/EL734Driver.h b/sinqEPICSApp/src/EL734Driver.h index 79bcccd..3e1f16c 100644 --- a/sinqEPICSApp/src/EL734Driver.h +++ b/sinqEPICSApp/src/EL734Driver.h @@ -34,9 +34,7 @@ private: double position; int homing; time_t next_poll; - int ErrTxtIdx; - - void forwardErrorTxt(EL734Axis *axis); + int errorReported; friend class EL734Controller; }; diff --git a/sinqEPICSApp/src/PhytronDriver.cpp b/sinqEPICSApp/src/PhytronDriver.cpp index 3c7123d..7163df8 100644 --- a/sinqEPICSApp/src/PhytronDriver.cpp +++ b/sinqEPICSApp/src/PhytronDriver.cpp @@ -60,7 +60,6 @@ PhytronController::PhytronController(const char *portName, const char *PhytronPo : SINQController(portName, PhytronPortName,2) { asynStatus status; - PhytronAxis *pAxis; static const char *functionName = "PhytronController::PhytronController"; char etx[2]; @@ -78,8 +77,8 @@ PhytronController::PhytronController(const char *portName, const char *PhytronPo pasynOctetSyncIO->setOutputEos(pasynUserController_,etx,strlen(etx)); pasynOctetSyncIO->setInputEos(pasynUserController_,etx,strlen(etx)); - pAxis = new PhytronAxis(this, 1, encX); - pAxis = new PhytronAxis(this, 2, encY); + new PhytronAxis(this, 1, encX); + new PhytronAxis(this, 2, encY); startPoller(1000./1000., IDLEPOLL, 2); } @@ -94,9 +93,7 @@ PhytronController::PhytronController(const char *portName, const char *PhytronPo extern "C" int PhytronCreateController(const char *portName, const char *PhytronPortName, const char *selector, int encX, int encY) { - PhytronController *pPhytronController - = new PhytronController(portName, PhytronPortName,selector, encX, encY); - pPhytronController = NULL; + new PhytronController(portName, PhytronPortName,selector, encX, encY); return(asynSuccess); } @@ -210,7 +207,6 @@ PhytronAxis::PhytronAxis(PhytronController *pC, int axisNo, int enc) } else { phytronChar = 'Y'; } - } @@ -265,7 +261,11 @@ asynStatus PhytronAxis::home(double minVelocity, double maxVelocity, double acce updateMsgTxtFromDriver(""); - sprintf(command, "%s%cO-",pC_->selector,phytronChar); + if(forwards){ + sprintf(command, "%s%cO+",pC_->selector,phytronChar); + } else { + sprintf(command, "%s%cO-",pC_->selector,phytronChar); + } homing = 1; next_poll= -1; status = pC_->transactController(axisNo_,command,reply); @@ -353,7 +353,7 @@ asynStatus PhytronAxis::setClosedLoop(bool closedLoop) * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ asynStatus PhytronAxis::poll(bool *moving) { - asynStatus comStatus; + asynStatus comStatus = asynSuccess; char command[COMLEN], reply[COMLEN]; double lowlim; @@ -364,6 +364,7 @@ asynStatus PhytronAxis::poll(bool *moving) return asynSuccess; } + setIntegerParam(pC_->motorStatusProblem_, false); // Read the current motor position if(encoder) { sprintf(command,"%s%cP22R",pC_->selector,phytronChar); @@ -372,13 +373,16 @@ asynStatus PhytronAxis::poll(bool *moving) } comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; - + if(comStatus == asynError) { + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("No connection to phytron controller"); + goto skip; + } if(strstr(reply,"NACK") != NULL){ setIntegerParam(pC_->motorStatusProblem_, true); errlogSevPrintf(errlogMajor, "Bad reply for position on %d", axisNo_); updateMsgTxtFromDriver("Bad reply reading position"); - goto skip; + goto skip; } /* read over the ACK @@ -392,7 +396,11 @@ asynStatus PhytronAxis::poll(bool *moving) // Read the moving status of this motor sprintf(command,"%s%c=H",pC_->selector,phytronChar); comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; + if(comStatus == asynError){ + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("No connection to phytron controller"); + goto skip; + } /* errlogPrintf("Axis %d, status reply %s, position %lf\n", axisNo_, reply, position); */ if(strstr(reply,"ACKN") != NULL){ *moving = true; @@ -416,10 +424,16 @@ asynStatus PhytronAxis::poll(bool *moving) */ sprintf(command,"%s%c=I+",pC_->selector,phytronChar); comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; + if(comStatus == asynError) { + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("No connection to phytron controller"); + goto skip; + } if(strstr(reply,"ACKE") != NULL){ setIntegerParam(pC_->motorStatusHighLimit_, true); - updateMsgTxtFromDriver("High Limit Hit"); + updateMsgTxtFromDriver("Hit High Limit"); + comStatus = asynError; + goto skip; } else { setIntegerParam(pC_->motorStatusHighLimit_, false); } @@ -429,10 +443,16 @@ asynStatus PhytronAxis::poll(bool *moving) */ sprintf(command,"%s%c=I-",pC_->selector,phytronChar); comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; + if(comStatus == asynError){ + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("No connection to phytron controller"); + goto skip; + } if(strstr(reply,"ACKE") != NULL){ setIntegerParam(pC_->motorStatusLowLimit_, true); updateMsgTxtFromDriver("Low Limit Hit"); + comStatus = asynError; + goto skip; } else { setIntegerParam(pC_->motorStatusLowLimit_, false); } @@ -442,11 +462,17 @@ asynStatus PhytronAxis::poll(bool *moving) */ sprintf(command,"%s%c=E",pC_->selector,phytronChar); comStatus = pC_->transactController(axisNo_,command,reply); - if(comStatus) goto skip; + if(comStatus == asynError) { + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("No connection to phytron controller"); + goto skip; + } if(strstr(reply,"ACKE") != NULL){ setIntegerParam(pC_->motorStatusProblem_, true); errlogSevPrintf(errlogMajor, "Electronics on %d", axisNo_); updateMsgTxtFromDriver("Electronics error"); + comStatus = asynError; + goto skip; } else { setIntegerParam(pC_->motorStatusProblem_, false); } @@ -455,9 +481,8 @@ asynStatus PhytronAxis::poll(bool *moving) skip: - setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0); callParamCallbacks(); - return comStatus ? asynError : asynSuccess; + return comStatus; } /** Code for iocsh registration */ diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index d821422..4359508 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -139,10 +139,20 @@ asynStatus pmacAxis::getAxisInitialStatus(void) sprintf(message,"scale_factor = %lf, high = %lf, low = %lf", scale_,high_limit, low_limit); pC_->debugFlow(message); - setDoubleParam(pC_->motorLowLimit_, low_limit); - setDoubleParam(pC_->motorHighLimit_, high_limit); + setIntegerParam(pC_->motorStatusHasEncoder_, 1); - + /* + In some configurations at SINQ, the high and low limits are interchanged in the motor controller. + This messy bit of code takes care of this messy electronics configuration. + */ + if(high_limit > low_limit){ + setDoubleParam(pC_->motorLowLimit_, low_limit); + setDoubleParam(pC_->motorHighLimit_, high_limit); + } else { + setDoubleParam(pC_->motorLowLimit_, high_limit); + setDoubleParam(pC_->motorHighLimit_, low_limit); + } + // 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);