diff --git a/iocBoot/iocsinqEPICS/euromovetest.cmd b/iocBoot/iocsinqEPICS/euromovetest.cmd index 7039110..565cd46 100755 --- a/iocBoot/iocsinqEPICS/euromovetest.cmd +++ b/iocBoot/iocsinqEPICS/euromovetest.cmd @@ -4,12 +4,9 @@ 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", "amor-ts:3002",0,0,0) diff --git a/sinqEPICSApp/Db/motorMessage.db b/sinqEPICSApp/Db/motorMessage.db index 2bbfd4a..6168c2b 100644 --- a/sinqEPICSApp/Db/motorMessage.db +++ b/sinqEPICSApp/Db/motorMessage.db @@ -6,4 +6,13 @@ record(waveform, "$(P)$(M)-MsgTxt") { field(FTVL, "CHAR") field(NELM, "80") field(SCAN, "I/O Intr") -} \ No newline at end of file +} + +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") +} diff --git a/sinqEPICSApp/src/EuroMoveDriver.cpp b/sinqEPICSApp/src/EuroMoveDriver.cpp index e085a91..e19819b 100644 --- a/sinqEPICSApp/src/EuroMoveDriver.cpp +++ b/sinqEPICSApp/src/EuroMoveDriver.cpp @@ -14,6 +14,7 @@ GPIB or USB. The controller is fairly simple. It can control quite a number of m #include #include #include +#include #include #include @@ -192,6 +193,44 @@ asynStatus EuroMoveController::transactController(int axisNo,char command[COMLEN return status; } + +asynStatus EuroMoveController::writeFloat64(asynUser *pasynUser, epicsFloat64 value) +{ + int function = pasynUser->reason; + asynStatus status = asynError; + EuroMoveAxis *pAxis = NULL; + + static const char *functionName = "EuroMoveController::writeFloat64"; + + + pAxis = (EuroMoveAxis *)this->getAxis(pasynUser); + if (!pAxis) { + return asynError; + } + + + /* Set the parameter and readback in the parameter library. */ + status = pAxis->setDoubleParam(function, value); + + errlogPrintf("%s, reason %d, value %f\n", functionName, function, value); + + // TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command + if (function == motorResolution_) { + if(value > .0){ + pAxis->resolution = (int)(1./value); + } else { + pAxis->resolution = 1; + } + } + + //Call base class method + //This will handle callCallbacks even if the function was handled here. + status = asynMotorController::writeFloat64(pasynUser, value); + + return status; + +} + // These are the EuroMoveAxis methods /** Creates a new EuroMoveAxis object. @@ -236,7 +275,7 @@ asynStatus EuroMoveAxis::move(double position, int relative, double minVelocity, if (relative) { position += this->position; } - homing = 0; + homingStatus = HomeIdle; sprintf(command,"G%d=%d", motNo, (int)position); status = pC_->transactController(motNo,command,reply); next_poll = -1; @@ -247,10 +286,29 @@ asynStatus EuroMoveAxis::home(double minVelocity, double maxVelocity, double acc { asynStatus status = asynSuccess; //static const char *functionName = "EuroMoveAxis::home"; + char command[COMLEN], reply[COMLEN]; /* - I do not know if this controller actually can do homing + The EuroMove controller does not have a built in homing mechanism. We simulate it by: + + 1). Running the motor in the appropriate end swicth at high speed + 2) Stepping back three units + 3) Run the motor again in the end switch at low speed for better accuracy + 4) Set the limit position with the command + + The state of this operation is maintained in the homingStatus variable */ + homingDirection = forwards; + homingStatus = HomeFastRun; + if(homingDirection){ + sprintf(command,"H%d=03", motNo); + } else { + sprintf(command,"H%d=07", motNo); + } + + status = pC_->transactController(motNo,command,reply); + next_poll = -1; + return status; } @@ -339,6 +397,7 @@ asynStatus EuroMoveAxis::poll(bool *moving) asynStatus comStatus = asynSuccess; char command[COMLEN], reply[COMLEN]; unsigned int axStatus; + double backStep, backPos, limit; // protect against excessive polling if(time(NULL) < next_poll){ @@ -371,47 +430,135 @@ asynStatus EuroMoveAxis::poll(bool *moving) /* errlogPrintf("Axis %d, status reply %s, position %d\n", axisNo_, reply, position); */ sscanf(reply, "%x", &axStatus); - if((axStatus & 128) > 0) { // test bit 8 - *moving = true; + if(homingStatus == HomeIdle){ + if((axStatus & 128) > 0) { // test bit 8 + *moving = true; + next_poll = -1; + setIntegerParam(pC_->motorStatusDone_, false); + } else { + *moving = false; + next_poll = time(NULL)+IDLEPOLL; + setIntegerParam(pC_->motorStatusDone_, true); + } + + /* Testing limit switches */ + if((axStatus & 2) > 0){ // bit 2 + setIntegerParam(pC_->motorStatusHighLimit_, true); + errlogPrintf("HighLimit detected on %d\n", motNo); + sendStop(); + } else { + setIntegerParam(pC_->motorStatusHighLimit_, false); + } + if((axStatus & 1) > 0){ // bit 1 + setIntegerParam(pC_->motorStatusLowLimit_, true); + errlogPrintf("LowLimit detected on %d\n", motNo); + sendStop(); + } else { + setIntegerParam(pC_->motorStatusLowLimit_, false); + } + + /* there could be errors reported in the motor status */ + if((axStatus & 8) > 0){ // bit 4 + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("Internal timeout detected"); + errlogPrintf("Internal timeout detected on %d\n", motNo); + comStatus = asynError; + } + if((axStatus & 4) > 0){ // bit 3 + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("Encoding anomaly"); + errlogPrintf("Encoding anomaly on %d\n", motNo); + comStatus = asynError; + } + } else { next_poll = -1; setIntegerParam(pC_->motorStatusDone_, false); - } else { - *moving = false; - next_poll = time(NULL)+IDLEPOLL; - setIntegerParam(pC_->motorStatusDone_, true); + *moving = true; + switch(homingStatus){ + case HomeIdle: + // handled above: this is here to silence the compiler + break; + case HomeFastRun: + // errlogPrintf("HomeFastRun\n"); + if(axStatus & 2 || axStatus & 1){ + sendStop(); + homingStatus = HomeFastStop; + } + break; + case HomeFastStop: + // errlogPrintf("HomeFastStop\n"); + if((axStatus & 128) == 0){ + backStep = 3 * resolution; + if(homingDirection){ + backPos = position - backStep; + } else { + backPos = position + backStep; + } + sprintf(command,"G%d=%d", motNo, (int)backPos); + comStatus = pC_->transactController(motNo,command,reply); + if(comStatus == asynError){ + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("Homing problem when back stepping"); + errlogPrintf("Homing problem when back stepping: %s not accepted\n", command); + *moving = false; + setIntegerParam(pC_->motorStatusAtHome_, true); + homingStatus = HomeIdle; + setIntegerParam(pC_->motorStatusDone_, true); + goto skip; + } + usleep(200); + homingStatus = HomeBackStep; + } + break; + case HomeBackStep: + //errlogPrintf("HomeBackStep\n"); + if((axStatus & 128) == 0){ + homingStatus = HomeSlowRun; + if(homingDirection){ + sprintf(command,"H%d=01", motNo); + } else { + sprintf(command,"H%d=05", motNo); + } + comStatus = pC_->transactController(motNo,command,reply); + if(comStatus == asynError){ + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("Homing problem slow running"); + errlogPrintf("Homing problem when slow running: %s not accepted\n", command); + *moving = false; + setIntegerParam(pC_->motorStatusAtHome_, true); + homingStatus = HomeIdle; + setIntegerParam(pC_->motorStatusDone_, true); + goto skip; + } + } + break; + case HomeSlowRun: + //errlogPrintf("HomeSlowRun\n"); + if(axStatus & 2 || axStatus & 1){ + sendStop(); + homingStatus = HomeSlowStop; + } + break; + case HomeSlowStop: + //errlogPrintf("HomeSlowStop\n"); + if((axStatus & 128) == 0) { + if(homingDirection) { + pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&limit); + } else { + pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&limit); + } + setPosition(limit); + *moving = false; + setIntegerParam(pC_->motorStatusDone_, true); + next_poll = time(NULL)+IDLEPOLL; + setIntegerParam(pC_->motorStatusDone_, true); + setIntegerParam(pC_->motorStatusAtHome_, true); + homingStatus = HomeIdle; + setDoubleParam(pC_->motorPosition_,(double)limit); + } + } } - /* Testing limit switches */ - if((axStatus & 2) > 0){ // bit 2 - setIntegerParam(pC_->motorStatusHighLimit_, true); - errlogPrintf("HighLimit detected on %d\n", motNo); - sendStop(); - } else { - setIntegerParam(pC_->motorStatusHighLimit_, false); - } - if((axStatus & 1) > 0){ // bit 1 - setIntegerParam(pC_->motorStatusLowLimit_, true); - errlogPrintf("LowLimit detected on %d\n", motNo); - sendStop(); - } else { - setIntegerParam(pC_->motorStatusLowLimit_, false); - } - - /* there could be errors reported in the motor status */ - if((axStatus & 8) > 0){ // bit 4 - setIntegerParam(pC_->motorStatusProblem_, true); - updateMsgTxtFromDriver("Internal timeout detected"); - errlogPrintf("Internal timeout detected on %d\n", motNo); - comStatus = asynError; - } - if((axStatus & 4) > 0){ // bit 3 - setIntegerParam(pC_->motorStatusProblem_, true); - updateMsgTxtFromDriver("Encoding anomaly"); - errlogPrintf("Encoding anomaly on %d\n", motNo); - comStatus = asynError; - } - - skip: callParamCallbacks(); return comStatus; diff --git a/sinqEPICSApp/src/EuroMoveDriver.h b/sinqEPICSApp/src/EuroMoveDriver.h index 38cff35..3bd41a7 100644 --- a/sinqEPICSApp/src/EuroMoveDriver.h +++ b/sinqEPICSApp/src/EuroMoveDriver.h @@ -12,6 +12,15 @@ January 2020 #define COMLEN 80 +typedef enum { + HomeIdle, + HomeFastRun, + HomeFastStop, + HomeBackStep, + HomeSlowRun, + HomeSlowStop, +} HomingStatus; + class EuroMoveAxis : public SINQAxis { public: @@ -35,9 +44,11 @@ private: time_t next_poll; int motNo; // motor number in the EuroMove controller int position; - int homing; + HomingStatus homingStatus; + int homingDirection; // 1 = forward, 0 backwards asynStatus sendStop(); -friend class EuroMoveController; + int resolution; + friend class EuroMoveController; }; class EuroMoveController : public SINQController { @@ -47,6 +58,7 @@ public: void report(FILE *fp, int level); EuroMoveAxis* getAxis(asynUser *pasynUser); EuroMoveAxis* getAxis(int axisNo); + asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value); friend class EuroMoveAxis; private: