From 4f7fe09f35b2da7316ba00b62721ae452cf7ce95 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Tue, 20 May 2014 12:26:44 +1000 Subject: [PATCH] Implement a turn counter for wrap-around rotary encoders --- .../TEST_SICS/fakeGalil/galilmotor.py | 6 + site_ansto/motor_dmc2280.c | 216 +++++++++++------- 2 files changed, 143 insertions(+), 79 deletions(-) diff --git a/site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py b/site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py index c4b3eb37..69499964 100644 --- a/site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py +++ b/site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py @@ -32,6 +32,7 @@ class GalilMotor(object): self.stopCallback = None self.moveCallback = None self.trace = False + self.rotary_bits = 0 def doTrace(self, arg): print "Trace %s: %s" % (self.axis, arg), @@ -80,6 +81,9 @@ class GalilMotor(object): elif lh == "NAM": self.name = rh[1:-1] self.configDone = True + elif lh == "ROT": + self.rotary_bits = int(rh) + self.configDone = True else: print "Unrecognized config item \"%s\" in \"%s\"" % (lh, arg) if self.configDone and not configDoneAlready: @@ -228,6 +232,8 @@ class GalilMotor(object): response = self.getPosition() elif arg == "TP": response = self.getEncoder() + if self.rotary_bits > 0: + response = response & ((1 << self.rotary_bits) - 1) elif arg == "SP": response = self.speed elif arg == "AC": diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 757586c6..ae8e9054 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -260,6 +260,8 @@ struct __MoDriv { #endif int bias_bits; /**< number of bits to mask */ int bias_bias; /**< bias to add to encoder value */ + int rotary_bits; /**< number of bits in rotary encoder */ + int rotary_count; /**< count of rotations */ char ao_id[256]; bool legacy_fsm; /**< flag for legacy_fsm new code */ bool doStats; /**< flag to request stats collection */ @@ -1541,7 +1543,7 @@ static void cmdConfig(pDMC2280Driv self) { } name[j] = '\0'; - snprintf(cmd, CMDLEN, "MG \"CONFIG%c=SPX=%g,CPX=%g,RL=%g,FL=%g,UH=%g,%s=%d,NAM='%s'\"", + snprintf(cmd, CMDLEN, "MG \"CONFIG%c=SPX=%g,CPX=%g,RL=%g,FL=%g,UH=%g,%s=%d,ROT=%d,NAM='%s'\"", self->axisLabel, self->stepsPerX, self->cntsPerX, @@ -1550,6 +1552,7 @@ static void cmdConfig(pDMC2280Driv self) { self->fHome, self->abs_encoder ? "EH" : "MH", self->abs_encoder ? self->absEncHome : self->motorHome, + self->rotary_bits, name); DMC_Send(self, cmd); } @@ -1729,6 +1732,21 @@ static int rspStatus(pDMC2280Driv self, const char* text) { self->currSteps = iSteps; if (self->bias_bits > 0) iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1); + if (self->rotary_bits > 0) { + int shift = 2; + int mask = (1 << shift) - 1; + int old_bits = (self->currCounts >> (self->rotary_bits - shift)) & mask; + int new_bits = (iCounts >> (self->rotary_bits - shift)) & mask; + if (old_bits == 0 && new_bits == mask) { + self->rotary_count -= 1; + SICSLogPrintf(eLog, "%s: Rotary count decrement to %d", self->name, self->rotary_count); + } + if (old_bits == mask && new_bits == 0) { + self->rotary_count += 1; + SICSLogPrintf(eLog, "%s: Rotary count increment to %d", self->name, self->rotary_count); + } + iCounts += self->rotary_count << self->rotary_bits; + } self->currCounts = iCounts; self->currPosition = motPosit(self); if (self->currPosition < self->minPosition) @@ -2150,6 +2168,10 @@ static bool motorHandleStatus(pDMC2280Driv self) { } if (checkPosition(self) == 0) { /* handle runaway */ + SICSLogPrintf(eWarning, "%s: Position overrun at %f (%d)", + self->name, + self->currPosition, + self->currCounts); self->errorCode = POSFAULT; /* recoverable fault */ self->faultPending = true; /* defer fault */ change_state(self, DMCState_MotorHalt); @@ -4333,6 +4355,14 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->bias_bits; return 1; } + if(strcasecmp(name,"rotary_count") == 0) { + *fValue = self->rotary_count; + return 1; + } + if(strcasecmp(name,"rotary_bits") == 0) { + *fValue = self->rotary_bits; + return 1; + } if(strcasecmp(name,"motorPollFast") == 0) { *fValue = self->motorPollFast; return 1; @@ -4742,6 +4772,20 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, self->bias_bits = newValue; return 1; } + if (self->abs_encoder && strcasecmp("rotary_count", name) == 0) { + /* Debug Managers only */ + if (!(self->debug && SCMatchRights(pCon, usMugger))) + return 0; + self->rotary_count = newValue; + return 1; + } + if (self->abs_encoder && strcasecmp("rotary_bits", name) == 0) { + /* Debug Managers only */ + if (!(self->debug && SCMatchRights(pCon, usMugger))) + return 0; + self->rotary_bits = newValue; + return 1; + } if (strcasecmp("motorPollFast", name) == 0) { /* Debug Managers only */ if (!(self->debug && SCMatchRights(pCon, usMugger))) @@ -5284,6 +5328,22 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { if (pNew->bias_bits < 0) pNew->bias_bits = -pNew->bias_bits; } + + /* Rotary count for encoder - add this to value read */ + if ((pPtr=getParam(pCon, interp, params,"rotary_count",_OPTIONAL)) == NULL) + pNew->rotary_count = 0; + else { + sscanf(pPtr, "%d", &(pNew->rotary_count)); + } + + /* Rotary size for encoder - mask this many bits */ + if ((pPtr=getParam(pCon, interp, params,"rotary_bits",_OPTIONAL)) == NULL) + pNew->rotary_bits = 0; + else { + sscanf(pPtr, "%d", &(pNew->rotary_bits)); + if (pNew->rotary_bits < 0) + pNew->rotary_bits = -pNew->rotary_bits; + } } if ((pPtr=getParam(pCon, interp, params,"posit_count",_OPTIONAL)) == NULL) @@ -5584,91 +5644,89 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, return 1; } } - else if(strcasecmp("driver", argv[1]) == 0) { - if(strcasecmp("run", argv[2]) == 0) { - if (argc > 2) { - float fLower, fUpper, fZero, fSign, fFixed, fLim; - double fSoft; - char *endPtr; - errno = 0; - fSoft = strtod(argv[3], &endPtr); - if (endPtr == argv[3] || errno == ERANGE) { + else if (strcasecmp("driver", argv[1]) == 0 && strcasecmp("run", argv[2]) == 0) { + if (argc > 2) { + float fLower, fUpper, fZero, fSign, fFixed, fLim; + double fSoft; + char *endPtr; + errno = 0; + fSoft = strtod(argv[3], &endPtr); + if (endPtr == argv[3] || errno == ERANGE) { + self->errorCode = BADPAR; + SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]); + return 1; + } + /* Reject Infinity and Not a Number (NaN) */ + switch (fpclassify(fSoft)) { + case FP_ZERO: + case FP_NORMAL: + break; + default: self->errorCode = BADPAR; SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]); return 1; - } - /* Reject Infinity and Not a Number (NaN) */ - switch (fpclassify(fSoft)) { - case FP_ZERO: - case FP_NORMAL: - break; - default: - self->errorCode = BADPAR; - SCPrintf(pCon, eWarning, "Motor %s bad run parameter %s", self->name, argv[3]); - return 1; - } - MotorGetPar(self->pMot, "softlowerlim", &fLower); - MotorGetPar(self->pMot, "softupperlim", &fUpper); - MotorGetPar(self->pMot, "softzero", &fZero); - MotorGetPar(self->pMot, "sign", &fSign); - MotorGetPar(self->pMot, "fixed", &fFixed); - if (fFixed >= 0) { - SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name); - } else if (fSoft > fUpper) { - SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s", - fSoft, fUpper, self->name); - } else if (fSoft < fLower) { - SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s", - fSoft, fLower, self->name); + } + MotorGetPar(self->pMot, "softlowerlim", &fLower); + MotorGetPar(self->pMot, "softupperlim", &fUpper); + MotorGetPar(self->pMot, "softzero", &fZero); + MotorGetPar(self->pMot, "sign", &fSign); + MotorGetPar(self->pMot, "fixed", &fFixed); + if (fFixed >= 0) { + SCPrintf(pCon, eWarning, "Motor %s is Fixed", self->name); + } else if (fSoft > fUpper) { + SCPrintf(pCon, eWarning, "%g violates upper software limit %g on %s", + fSoft, fUpper, self->name); + } else if (fSoft < fLower) { + SCPrintf(pCon, eWarning, "%g violates lower software limit %g on %s", + fSoft, fLower, self->name); + } else { + double fHard = fSoft; + fHard += fZero; + fHard *= fSign; + if (fHard > self->fUpper) { + SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s", + fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name); + } else if (fHard < self->fLower) { + SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s", + fSoft, self->fLower * fSign + fZero, self->fLower, self->name); } else { - double fHard = fSoft; - fHard += fZero; - fHard *= fSign; - if (fHard > self->fUpper) { - SCPrintf(pCon, eWarning, "%g violates upper hardware limit %g (%g) on %s", - fSoft, self->fUpper * fSign + fZero, self->fUpper, self->name); - } else if (fHard < self->fLower) { - SCPrintf(pCon, eWarning, "%g violates lower hardware limit %g (%g) on %s", - fSoft, self->fLower * fSign + fZero, self->fLower, self->name); - } else { - self->fTarget = fHard; - SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); - self->doOscillate = false; - self->doReportMotion = true; - state_cmd_execute(self, CMD_RUN); - } + self->fTarget = fHard; + SCPrintf(pCon, eLog, "Running %s to soft=%g, hard=%g", self->name, fSoft, fHard); + self->doOscillate = false; + self->doReportMotion = true; + state_cmd_execute(self, CMD_RUN); } } - else { - char *txt = NULL; - if (OKOK == self->driver_status) - txt = "OKOK"; - else if (HWIdle == self->driver_status) - txt = "HWIdle"; - else if (HWBusy == self->driver_status) - txt = "HWBusy"; - else if (HWFault == self->driver_status) - txt = "HWFault"; - else if (HWPosFault == self->driver_status) - txt = "HWPosFault"; - else if (HWCrash == self->driver_status) - txt = "HWCrash"; - else if (NOMEMORY == self->driver_status) - txt = "NOMEMORY"; - else if (HWNoBeam == self->driver_status) - txt = "HWNoBeam"; - else if (HWPause == self->driver_status) - txt = "HWPause"; - else if (HWWarn == self->driver_status) - txt = "HWWarn"; - else if (HWRedo == self->driver_status) - txt = "HWRedo"; - else - txt = "HWUnknown"; - SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt); - } - return 1; } + else { + char *txt = NULL; + if (OKOK == self->driver_status) + txt = "OKOK"; + else if (HWIdle == self->driver_status) + txt = "HWIdle"; + else if (HWBusy == self->driver_status) + txt = "HWBusy"; + else if (HWFault == self->driver_status) + txt = "HWFault"; + else if (HWPosFault == self->driver_status) + txt = "HWPosFault"; + else if (HWCrash == self->driver_status) + txt = "HWCrash"; + else if (NOMEMORY == self->driver_status) + txt = "NOMEMORY"; + else if (HWNoBeam == self->driver_status) + txt = "HWNoBeam"; + else if (HWPause == self->driver_status) + txt = "HWPause"; + else if (HWWarn == self->driver_status) + txt = "HWWarn"; + else if (HWRedo == self->driver_status) + txt = "HWRedo"; + else + txt = "HWUnknown"; + SCPrintf(pCon, eValue, "%s.run = %s", self->name, txt); + } + return 1; } else if(strcasecmp("status", argv[1]) == 0) { char *txt = NULL;