cleanups and add encoder bias code
r2592 | dcl | 2008-05-29 14:52:41 +1000 (Thu, 29 May 2008) | 2 lines
This commit is contained in:
Douglas Clowes
@ -198,9 +198,6 @@ struct __MoDriv {
|
||||
pNWTimer state_timer; /**< motor state timer */
|
||||
SConnection *trace;
|
||||
int posit_count;
|
||||
#ifdef POSIT_COUNTS
|
||||
long long* posit_array;
|
||||
#endif
|
||||
double* positions;
|
||||
int variables;
|
||||
#if defined(STATE_TRACE) && (STATE_TRACE > 0)
|
||||
@ -208,6 +205,8 @@ struct __MoDriv {
|
||||
char state_trace_text[STATE_TRACE][CMDLEN];
|
||||
struct timeval state_trace_time[STATE_TRACE];
|
||||
#endif
|
||||
int bias_bits; /**< number of bits to mask */
|
||||
int bias_bias; /**< bias to add to encoder value */
|
||||
};
|
||||
|
||||
int DMC2280MotionControl = 1; /* defaults to enabled */
|
||||
@ -283,70 +282,6 @@ static int DMC2280Halt(void *pData);
|
||||
static int DMC2280SetPar(void *pData, SConnection *pCon,
|
||||
char *name, float newValue);
|
||||
|
||||
#ifdef POSIT_COUNTS
|
||||
static bool check_posits(pDMC2280Driv self) {
|
||||
char line[CMDLEN];
|
||||
int missing = 0;
|
||||
int direction = 0;
|
||||
int i;
|
||||
|
||||
if (self->posit_count < 2) {
|
||||
snprintf(line, ERRLEN, "Insufficient positions on motor '%s': %d",
|
||||
self->name, self->posit_count);
|
||||
SICSLogWrite(line,eError);
|
||||
return false;
|
||||
}
|
||||
for (i = 0; i < self->posit_count; ++i)
|
||||
if (self->posit_array[i] < 0)
|
||||
++missing;
|
||||
if (missing) {
|
||||
snprintf(line, ERRLEN, "Missing positions on motor '%s': %d",
|
||||
self->name, missing);
|
||||
SICSLogWrite(line,eError);
|
||||
return false;
|
||||
}
|
||||
for (i = 1; i < self->posit_count; ++i) {
|
||||
if (i == 1) {
|
||||
if (self->posit_array[i] > self->posit_array[i - 1])
|
||||
direction = 1;
|
||||
else if (self->posit_array[i] < self->posit_array[i - 1])
|
||||
direction = -1;
|
||||
else
|
||||
direction = 0;
|
||||
}
|
||||
if (direction == 0) {
|
||||
snprintf(line, ERRLEN, "Position order on motor '%s' : %d",
|
||||
self->name, i);
|
||||
SICSLogWrite(line,eError);
|
||||
}
|
||||
else {
|
||||
switch (direction) {
|
||||
case -1:
|
||||
if (!(self->posit_array[i] < self->posit_array[i - 1])) {
|
||||
direction = 0;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (!(self->posit_array[i] > self->posit_array[i - 1])) {
|
||||
direction = 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (direction != 0)
|
||||
return true;
|
||||
for (i = 0; i < self->posit_count; ++i) {
|
||||
snprintf(line, ERRLEN, "Position %2d motor '%s' : %lld",
|
||||
i + 1,
|
||||
self->name,
|
||||
self->posit_array[i]);
|
||||
/* TODO log me */
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
static bool check_positions(pDMC2280Driv self) {
|
||||
char line[CMDLEN];
|
||||
int missing = 0;
|
||||
@ -452,88 +387,6 @@ static double count2unit(pDMC2280Driv self, long long counts) {
|
||||
return fPos;
|
||||
}
|
||||
|
||||
#ifdef POSIT_COUNTS
|
||||
static long long posit2count(pDMC2280Driv self, double target) {
|
||||
double absolute;
|
||||
long long result;
|
||||
if (self->abs_encoder == 0) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "posit2count motor %s has no absolute encoder",
|
||||
self->name);
|
||||
SICSLogWrite(line, eStatus);
|
||||
return -1;
|
||||
}
|
||||
if (!check_posits(self))
|
||||
return -1;
|
||||
|
||||
int i = ((int) target) - 1;
|
||||
if (i < 0) {
|
||||
i = 0;
|
||||
} else if (i > self->posit_count - 2) {
|
||||
i = self->posit_count - 2;
|
||||
}
|
||||
absolute = (double)self->posit_array[i]
|
||||
+ ((double)target - ((double)i + 1)) *
|
||||
((double)self->posit_array[i + 1] - self->posit_array[i]);
|
||||
if (absolute > 0)
|
||||
result = (int) (absolute + 0.5);
|
||||
else if (absolute < 0)
|
||||
result = (int) (absolute - 0.5);
|
||||
else
|
||||
result = (int) absolute;
|
||||
if (self->debug) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "posit2count Rounding %f to %lld", absolute, result);
|
||||
SICSLogWrite(line, eStatus);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
static double count2posit(pDMC2280Driv self, long long counts) {
|
||||
int i, j;
|
||||
double fPos;
|
||||
if (self->abs_encoder == 0) {
|
||||
char line[CMDLEN];
|
||||
snprintf(line, CMDLEN, "count2posit motor %s has no absolute encoder",
|
||||
self->name);
|
||||
SICSLogWrite(line, eStatus);
|
||||
return 0;
|
||||
}
|
||||
if (!check_posits(self))
|
||||
return 0;
|
||||
|
||||
if (self->posit_array[0] < self->posit_array[1]) {
|
||||
for (i = 1; i < self->posit_count - 1; ++i) {
|
||||
if (counts < self->posit_array[i]) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
for (i = 1; i < self->posit_count - 1; ++i) {
|
||||
if (counts > self->posit_array[i]) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
--i;
|
||||
j = i + 1;
|
||||
fPos = (double) j +
|
||||
((double)counts - self->posit_array[i]) /
|
||||
((double)self->posit_array[j] - self->posit_array[i]);
|
||||
return fPos;
|
||||
}
|
||||
|
||||
static double unit2posit(pDMC2280Driv self, double target) {
|
||||
return count2posit(self, unit2count(self, target));
|
||||
}
|
||||
|
||||
static double posit2unit(pDMC2280Driv self, double target) {
|
||||
return count2unit(self, posit2count(self, target));
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
static double unit2posit(pDMC2280Driv self, double target) {
|
||||
int i, j;
|
||||
double fPos;
|
||||
@ -584,8 +437,6 @@ static double count2posit(pDMC2280Driv self, long long counts) {
|
||||
return unit2posit(self, count2unit(self, counts));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/** \brief Convert axis speed in physical units to
|
||||
* motor speed in steps/sec.
|
||||
*
|
||||
@ -1026,7 +877,10 @@ static int readAbsEnc(pDMC2280Driv self, long *pos) {
|
||||
if (FAILURE == DMC_SendReceive(self, cmd, reply))
|
||||
return FAILURE;
|
||||
|
||||
*pos = atol(reply);
|
||||
long iCounts = atol(reply);
|
||||
if (self->bias_bits > 0 && self->bias_bias != 0)
|
||||
iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1);
|
||||
*pos = iCounts;
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
@ -1446,6 +1300,8 @@ static int rspStatus(pDMC2280Driv self, const char* text) {
|
||||
}
|
||||
self->currFlags = iFlags;
|
||||
self->currSteps = iSteps;
|
||||
if (self->bias_bits > 0 && self->bias_bias != 0)
|
||||
iCounts = (iCounts + self->bias_bias) & ((1 << self->bias_bits) - 1);
|
||||
self->currCounts = iCounts;
|
||||
self->currPosition = motPosit(self);
|
||||
self->thread0 = iXQ0;
|
||||
@ -1777,7 +1633,6 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
|
||||
/* handle pending message event */
|
||||
if (self->waitResponse) {
|
||||
self->subState = 0;
|
||||
/* FIXME: FIXME_DOUG */
|
||||
return;
|
||||
}
|
||||
/* Set speed */
|
||||
@ -2521,7 +2376,7 @@ static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
|
||||
case eCommandEvent:
|
||||
switch (event->event.cmd.cmd_type) {
|
||||
case CMD_RUN:
|
||||
/* TODO: FIXME RUN command while running */
|
||||
/* RUN command while running */
|
||||
if (self->driver_status == HWIdle)
|
||||
self->driver_status = HWBusy;
|
||||
self->run_flag = 1;
|
||||
@ -2966,7 +2821,7 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
|
||||
strncpy(error, self->dmc2280Error, (size_t)errLen);
|
||||
break;
|
||||
default:
|
||||
/* FIXME What's the default */
|
||||
/* What's the default */
|
||||
snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
|
||||
break;
|
||||
}
|
||||
@ -3143,6 +2998,14 @@ static int DMC2280GetPar(void *pData, char *name,
|
||||
*fValue = self->backlash_offset;
|
||||
return 1;
|
||||
}
|
||||
if(strcasecmp(name,"bias_bias") == 0) {
|
||||
*fValue = self->bias_bias;
|
||||
return 1;
|
||||
}
|
||||
if(strcasecmp(name,"bias_bits") == 0) {
|
||||
*fValue = self->bias_bits;
|
||||
return 1;
|
||||
}
|
||||
if (self->posit_count > 0) {
|
||||
if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) {
|
||||
int index;
|
||||
@ -3453,26 +3316,39 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
|
||||
self->fUpper = newValue;
|
||||
return 1;
|
||||
}
|
||||
if (self->abs_encoder && strcasecmp("stepsPerX", name) == 0) {
|
||||
if (strcasecmp("stepsPerX", name) == 0) {
|
||||
/* Debug Managers only */
|
||||
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
||||
return 0;
|
||||
self->stepsPerX = newValue;
|
||||
return 1;
|
||||
}
|
||||
if (self->debug && strcasecmp("cntsPerX", name) == 0) {
|
||||
if (self->abs_encoder && strcasecmp("cntsPerX", name) == 0) {
|
||||
/* Debug Managers only */
|
||||
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
||||
return 0;
|
||||
self->cntsPerX = newValue;
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (self->abs_encoder && strcasecmp("bias_bias", name) == 0) {
|
||||
/* Debug Managers only */
|
||||
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
||||
return 0;
|
||||
self->bias_bias = newValue;
|
||||
return 1;
|
||||
}
|
||||
if (self->abs_encoder && strcasecmp("bias_bits", name) == 0) {
|
||||
/* Debug Managers only */
|
||||
if (!(self->debug && SCMatchRights(pCon, usMugger)))
|
||||
return 0;
|
||||
self->bias_bits = newValue;
|
||||
return 1;
|
||||
}
|
||||
/* XXX Maybe move this to a configuration parameter.*/
|
||||
/* Set home, managers only. Users should set softposition */
|
||||
if(strcasecmp(name, HOME) == 0) {
|
||||
/* Debug Managers only */
|
||||
if(!(self->debug && SCMatchRights(pCon,usMugger)))
|
||||
if(!SCMatchRights(pCon,usMugger))
|
||||
return 1;
|
||||
else {
|
||||
if ( (self->fLower - newValue) > FLT_EPSILON) {
|
||||
@ -3496,9 +3372,6 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
|
||||
index = strtol(&name[6], NULL, 10);
|
||||
if (index < 1 || index > self->posit_count)
|
||||
return 0;
|
||||
#ifdef POSIT_COUNTS
|
||||
self->posit_array[index - 1] = newValue;
|
||||
#endif
|
||||
self->positions[index - 1] = count2unit(self, newValue);
|
||||
return 1;
|
||||
}
|
||||
@ -3641,12 +3514,6 @@ static void KillDMC2280(/*@only@*/void *pData){
|
||||
AsyncUnitDestroy(self->asyncUnit);
|
||||
self->asyncUnit = NULL;
|
||||
}
|
||||
#ifdef POSIT_COUNTS
|
||||
if (self->posit_array) {
|
||||
free(self->posit_array);
|
||||
self->posit_array = NULL;
|
||||
}
|
||||
#endif
|
||||
if (self->positions) {
|
||||
free(self->positions);
|
||||
self->positions = 0;
|
||||
@ -3929,37 +3796,44 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
|
||||
pNew->creep_precision = -pNew->creep_precision;
|
||||
}
|
||||
|
||||
if ((pPtr=getParam(pCon, interp, params,"posit_count",_OPTIONAL)) == NULL)
|
||||
pNew->posit_count = 0;
|
||||
/* BIAS for encoder - add this to value read */
|
||||
if ((pPtr=getParam(pCon, interp, params,"bias_bias",_OPTIONAL)) == NULL)
|
||||
pNew->bias_bias = 0;
|
||||
else {
|
||||
sscanf(pPtr,"%d",&(pNew->posit_count));
|
||||
if (pNew->posit_count < 1 || pNew->posit_count > 99) {
|
||||
snprintf(pError, ERRLEN, "Invalid posit_count on motor '%s': %d", motor,
|
||||
pNew->posit_count);
|
||||
SCWrite(pCon,pError,eError);
|
||||
pNew->posit_count = 0;
|
||||
}
|
||||
else {
|
||||
int i;
|
||||
char line[80];
|
||||
#ifdef POSIT_COUNTS
|
||||
pNew->posit_array = malloc(pNew->posit_count * sizeof(*pNew->posit_array));
|
||||
#endif
|
||||
pNew->positions = malloc(pNew->posit_count * sizeof(*pNew->positions));
|
||||
for (i = 0; i < pNew->posit_count; ++i) {
|
||||
snprintf(line, 80, "posit_%d", i + 1);
|
||||
if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) {
|
||||
#ifdef POSIT_COUNTS
|
||||
pNew->posit_array[i] = -1;
|
||||
#endif
|
||||
pNew->positions[i] = 0.0;
|
||||
}
|
||||
else {
|
||||
#ifdef POSIT_COUNTS
|
||||
pNew->posit_array[i] = strtol(pPtr, NULL, 10);
|
||||
#endif
|
||||
pNew->positions[i] = count2unit(pNew, strtol(pPtr, NULL, 10));
|
||||
}
|
||||
sscanf(pPtr, "%d", &(pNew->bias_bias));
|
||||
}
|
||||
|
||||
/* BIAS for encoder - mask this many bits */
|
||||
if ((pPtr=getParam(pCon, interp, params,"bias_bits",_OPTIONAL)) == NULL)
|
||||
pNew->bias_bits = 0.0;
|
||||
else {
|
||||
sscanf(pPtr, "%d", &(pNew->bias_bits));
|
||||
if (pNew->bias_bits < 0)
|
||||
pNew->bias_bits = -pNew->bias_bits;
|
||||
}
|
||||
}
|
||||
|
||||
if ((pPtr=getParam(pCon, interp, params,"posit_count",_OPTIONAL)) == NULL)
|
||||
pNew->posit_count = 0;
|
||||
else {
|
||||
sscanf(pPtr,"%d",&(pNew->posit_count));
|
||||
if (pNew->posit_count < 1 || pNew->posit_count > 99) {
|
||||
snprintf(pError, ERRLEN, "Invalid posit_count on motor '%s': %d", motor,
|
||||
pNew->posit_count);
|
||||
SCWrite(pCon,pError,eError);
|
||||
pNew->posit_count = 0;
|
||||
}
|
||||
else {
|
||||
int i;
|
||||
char line[80];
|
||||
pNew->positions = malloc(pNew->posit_count * sizeof(*pNew->positions));
|
||||
for (i = 0; i < pNew->posit_count; ++i) {
|
||||
snprintf(line, 80, "posit_%d", i + 1);
|
||||
if ((pPtr=getParam(pCon, interp, params,line,_OPTIONAL)) == NULL) {
|
||||
pNew->positions[i] = 0.0;
|
||||
}
|
||||
else {
|
||||
pNew->positions[i] = count2unit(pNew, strtol(pPtr, NULL, 10));
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -4217,12 +4091,6 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
if (self->posit_count > 0) {
|
||||
self->posit_count = 0;
|
||||
}
|
||||
#ifdef POSIT_COUNTS
|
||||
if (self->posit_array) {
|
||||
free(self->posit_array);
|
||||
self->posit_array = NULL;
|
||||
}
|
||||
#endif
|
||||
if (self->positions) {
|
||||
free(self->positions);
|
||||
self->positions = NULL;
|
||||
@ -4236,15 +4104,9 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
}
|
||||
self->posit_count = argc - 2;
|
||||
self->positions = malloc(self->posit_count * sizeof(*self->positions));
|
||||
#ifdef POSIT_COUNTS
|
||||
self->posit_array = malloc(self->posit_count * sizeof(*self->posit_array));
|
||||
#endif
|
||||
int i;
|
||||
for (i = 0; i < self->posit_count; ++i) {
|
||||
self->positions[i] = strtod(argv[i + 2], NULL);
|
||||
#ifdef POSIT_COUNTS
|
||||
self->posit_array[i] = unit2count(self, self->positions[i]);
|
||||
#endif
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user