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

66
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,8 +5644,7 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
return 1;
}
}
else if(strcasecmp("driver", argv[1]) == 0) {
if(strcasecmp("run", argv[2]) == 0) {
else if (strcasecmp("driver", argv[1]) == 0 && strcasecmp("run", argv[2]) == 0) {
if (argc > 2) {
float fLower, fUpper, fZero, fSign, fFixed, fLim;
double fSoft;
@ -5669,7 +5728,6 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
}
return 1;
}
}
else if(strcasecmp("status", argv[1]) == 0) {
char *txt = NULL;
if (OKOK == self->driver_status)