From a4d7cb12cc20b80fb1b2c81d47432181497e36cb Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Wed, 17 Oct 2007 08:36:19 +1000 Subject: [PATCH] Begin migration of motor positions from encoder counts to motor units (mm/degree) r2185 | dcl | 2007-10-17 08:36:19 +1000 (Wed, 17 Oct 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 220 ++++++++++++++++++++++++++++++++++--- 1 file changed, 204 insertions(+), 16 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 60a0466c..eb1d1a3a 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -183,8 +183,10 @@ struct __MoDriv { pNWTimer state_timer; /**< motor state timer */ SConnection *trace; int posit_count; - bool posit_check; - unsigned long long *posit_array; +#ifdef POSIT_COUNTS + long long* posit_array; +#endif + double* positions; int variables; }; @@ -251,6 +253,7 @@ 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; @@ -308,6 +311,70 @@ static bool check_posits(pDMC2280Driv self) { 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; + 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->positions[i] < -9999999) + ++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->positions[i] > self->positions[i - 1]) + direction = 1; + else if (self->positions[i] < self->positions[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->positions[i] < self->positions[i - 1])) { + direction = 0; + } + break; + case 1: + if (!(self->positions[i] > self->positions[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' : %f", + i + 1, + self->name, + self->positions[i]); + /* TODO log me */ } return false; } @@ -355,6 +422,7 @@ static float count2unit(pDMC2280Driv self, long long counts) { return fPos; } +#ifdef POSIT_COUNTS static long long posit2count(pDMC2280Driv self, float target) { double absolute; long long result; @@ -365,6 +433,9 @@ static long long posit2count(pDMC2280Driv self, float target) { SICSLogWrite(line, eStatus); return -1; } + if (!check_posits(self)) + return -1; + int i = ((int) target) - 1; if (i < 0) { i = 0; @@ -431,6 +502,60 @@ static float posit2unit(pDMC2280Driv self, float target) { return count2unit(self, posit2count(self, target)); } +#else + +static float unit2posit(pDMC2280Driv self, float target) { + int i, j; + float fPos; + if (!check_positions(self)) + return 0; + + if (self->positions[0] < self->positions[1]) { + for (i = 1; i < self->posit_count - 1; ++i) { + if (target < self->positions[i]) { + break; + } + } + } + else { + for (i = 1; i < self->posit_count - 1; ++i) { + if (target > self->positions[i]) { + break; + } + } + } + --i; + j = i + 1; + fPos = (double) j + + ((double)target - self->positions[i]) / + ((double)self->positions[j] - self->positions[i]); + return fPos; +} + +static float posit2unit(pDMC2280Driv self, float target) { + double result; + int i = ((int) target) - 1; + if (i < 0) { + i = 0; + } else if (i > self->posit_count - 2) { + i = self->posit_count - 2; + } + result = (double)self->positions[i] + + ((double)target - ((double)i + 1)) * + ((double)self->positions[i + 1] - self->positions[i]); + return result; +} + +static long long posit2count(pDMC2280Driv self, float target) { + return unit2count(self, posit2unit(self, target)); +} + +static float 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. * @@ -2485,6 +2610,17 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->backlash_offset; return 1; } + if (self->posit_count > 0) { + if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) { + int index; + *fValue = -1.0; + index = strtol(&name[6], NULL, 10); +// if (index < 1 || index > self->posit_count) +// return 0; + *fValue = posit2count(self, index); + return 1; + } + } if (self->abs_encoder != 0) { if (strcasecmp(name,"absenc") == 0) { if (readAbsEnc(self, fValue) == SUCCESS) @@ -2516,18 +2652,6 @@ static int DMC2280GetPar(void *pData, char *name, } } - if (self->posit_count > 0) { - int index; - if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) { - *fValue = -1.0; - index = strtol(&name[6], NULL, 10); - if (index < 1 || index > self->posit_count) - return 0; - *fValue = (float) self->posit_array[index - 1]; - return 1; - } - } - if(strcasecmp(name,"thread0") == 0) return readThread(self, 0, fValue); if(strcasecmp(name,"thread1") == 0) @@ -2798,7 +2922,10 @@ 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; } } @@ -2886,7 +3013,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); for (i = 0; i < self->posit_count; ++i) { snprintf(buffer, BUFFLEN, "%s.posit_%d = %lld\n", name, i + 1, - self->posit_array[i]); + posit2count(self, i + 1)); SCWrite(pCon, buffer, eStatus); } } @@ -2945,11 +3072,17 @@ 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; - self->posit_count = 0; } +#endif + if (self->positions) { + free(self->positions); + self->positions = 0; + } + self->posit_count = 0; /* Not required as performed in caller * free(self); */ @@ -3238,14 +3371,23 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { 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)); } } } @@ -3456,6 +3598,52 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, SCWrite(pCon, line, eValue); return 1; } + else if(strcasecmp("positions", argv[1]) == 0) { + if (argc == 2) { + int i, k = 0; + char line[1320]; + k += snprintf(line, sizeof(line) - k, "%s.positions ", self->name); + for (i = 0; i < self->posit_count; ++i) { + k += snprintf(&line[k], sizeof(line) - k, " %f", + self->positions[i]); + } + SCWrite(pCon, line, eValue); + return 1; + } + 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; + self->posit_count = 0; + } + if (argc > 3 && strcasecmp("erase", argv[2]) == 0) { + char line[132]; + snprintf(line, 132, "%s.posit_count = %d", self->name, self->posit_count); + SCWrite(pCon, line, eValue); + return 1; + } + 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; + } if (self->posit_count > 0) { if(strcasecmp("posit2count", argv[1]) == 0) { char line[132];