diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 38dde655..813d2576 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -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; }