linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implement a turn counter for wrap-around rotary encoders

Browse Source
This commit is contained in:
Douglas Clowes
2014-05-20 12:26:44 +10:00
parent 1a28faabbf
commit 4f7fe09f35
2 changed files with 143 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py
View File

@ -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":

216
site_ansto/motor_dmc2280.c
View File

@ -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;