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

cleanups and add encoder bias code

Browse Source 
r2592 | dcl | 2008-05-29 14:52:41 +1000 (Thu, 29 May 2008) | 2 lines
This commit is contained in:
Douglas Clowes
2008-05-29 14:52:41 +10:00
parent 9f479bc136
commit a0684a033e
1 changed files with 72 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
site_ansto/motor_dmc2280.c
View File

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