diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index c6826674..4685cb26 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -1,3 +1,4 @@ +#define POSIT_SHIM /** \file motor_dmc2280.c * \brief Driver for Galil DMC2280 motor controller. * @@ -125,7 +126,7 @@ struct __MoDriv { char axisLabel; char lastCmd[CMDLEN]; char dmc2280Error[CMDLEN]; - float home; /**< home position for axis, default=0 */ + float fHome; /**< home position for axis, default=0 */ int motorHome; /**< motor home position in steps */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */ float stepsPerX; /**< steps per physical unit */ @@ -138,8 +139,8 @@ struct __MoDriv { int currCounts; float currPosition; /**< Position at last position check */ float lastPosition; /**< Position at last position check */ - float lastSteps; - float lastCounts; + int lastSteps; + int lastCounts; int thread0; /**< last read of _XQ0 */ unsigned short int input0; /**< last read of _TI0 */ unsigned short int input1; /**< last read of _TI1 */ @@ -175,6 +176,9 @@ struct __MoDriv { int subState; pNWTimer state_timer; /**< motor state timer */ SConnection *trace; + int posit_count; + bool posit_check; + unsigned long long *posit_array; }; int DMC2280MotionControl = 1; /* defaults to enabled */ @@ -240,6 +244,186 @@ static int DMC2280Halt(void *pData); static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue); +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]); + } + return false; +} + +static long long unit2count(pDMC2280Driv self, float target) { + double absolute; + long long result; + if (self->abs_encoder == 0) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder", + self->name); + SICSLogWrite(line, eStatus); + return -1; + } + /* distance of target from home in units */ + absolute = (double) target - self->fHome; + /* convert to encoder counts */ + absolute *= (double) self->cntsPerX; + /* add home position in counts */ + absolute += (double) self->absEncHome; + 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, "unit2count Rounding %f to %lld", absolute, result); + SICSLogWrite(line, eStatus); + } + return result; +} + +static float count2unit(pDMC2280Driv self, long long counts) { + float fPos; + if (self->abs_encoder == 0) { + char line[CMDLEN]; + snprintf(line, CMDLEN, "unit2count motor %s has no absolute encoder", + self->name); + SICSLogWrite(line, eStatus); + return -1; + } + fPos = (counts - self->absEncHome) / self->cntsPerX + self->fHome; + return fPos; +} + +static long long posit2count(pDMC2280Driv self, float 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; + } + 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 float count2posit(pDMC2280Driv self, long long counts) { + int i, j; + float 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 float unit2posit(pDMC2280Driv self, float target) { + return count2posit(self, unit2count(self, target)); +} + +static float posit2unit(pDMC2280Driv self, float target) { + return count2unit(self, posit2count(self, target)); +} + /** \brief Convert axis speed in physical units to * motor speed in steps/sec. * @@ -288,12 +472,13 @@ static int motDecel(pDMC2280Driv self, float axisDecel) { static float motPosit(pDMC2280Driv self) { float fPos; if (self->abs_encoder) - fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->home; + fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->fHome; else - fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->home; + fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->fHome; return fPos; } + /** \brief Convert motor target in physical units to * motor absolute position in steps * @@ -305,24 +490,17 @@ static int motAbsol(pDMC2280Driv self, float target) { double absolute; int result; if (self->abs_encoder) { -#if 1 /* distance of target from home in units */ - absolute = target - self->home; + absolute = target - self->fHome; /* subtract current encoder position in units */ absolute -= (self->currCounts - self->absEncHome) / self->cntsPerX; /* convert to motor steps */ absolute *= self->stepsPerX; /* add current position in steps */ absolute += (double) self->currSteps; -#else - absolute = ((self->absEncHome - self->currCounts) + - (self->cntsPerX * (target - self->home))) * - (self->stepsPerX / self->cntsPerX) + - self->currSteps; -#endif } else { - absolute = (target - self->home) * self->stepsPerX + self->motorHome; + absolute = (target - self->fHome) * self->stepsPerX + self->motorHome; } if (absolute > 0) result = (int) (absolute + 0.5); @@ -768,7 +946,7 @@ static int RunHomeRoutine(pDMC2280Driv self, float newValue) { * - 0 MOTOR BLOCKED, no significant change in position detected. */ static int checkMotion(void *pData) { - float steps, counts, ratio_obs, ratio_exp, ratio_cmp; + float ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; @@ -780,11 +958,7 @@ static int checkMotion(void *pData) { return 1; if (self->time_lastPos_set.tv_sec == 0) { /* first time - initialise the data */ - if (self->has_fsm) { - steps = self->currSteps; - counts = self->currCounts; - } - set_lastMotion(pData, steps, counts); + set_lastMotion(self, self->currSteps, self->currCounts); return 1; } gettimeofday(&now, NULL); @@ -794,21 +968,17 @@ static int checkMotion(void *pData) { usec_TimeDiff -= self->time_lastPos_set.tv_usec; if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval)) return 1; - if (self->has_fsm) { - steps = self->currSteps; - counts = self->currCounts; - } /* If not stepping, then not blocked */ - if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { + if (fabs(self->currSteps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) { /* just update the timestamp */ - set_lastMotion(pData, self->lastSteps, self->lastCounts); + set_lastMotion(self, self->lastSteps, self->lastCounts); return 1; } /* calculate observed and expected steps per count ratios */ - if (counts == self->lastCounts) /* prevent divide by zero */ - ratio_obs = (steps - self->lastSteps); + if (self->currCounts == self->lastCounts) /* prevent divide by zero */ + ratio_obs = (self->currSteps - self->lastSteps); else - ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts); + ratio_obs = (self->currSteps - self->lastSteps) / (self->currCounts - self->lastCounts); ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX; ratio_cmp = ratio_obs / ratio_exp; /* wrong signs, less than half, or more than double is trouble */ @@ -822,13 +992,14 @@ static int checkMotion(void *pData) { snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f", self->name, ratio_obs, ratio_exp, cmp); SICSLogWrite(msg, eError); - snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%f/%f", - steps, self->lastSteps, counts, self->lastCounts, + snprintf(msg, sizeof(msg), "steps=%d-%d, counts=%d-%d, exp=%f/%f", + self->currSteps, self->lastSteps, + self->currCounts, self->lastCounts, self->stepsPerX, self->cntsPerX); SICSLogWrite(msg, eError); if (self->blockage_fail) return 0; - set_lastMotion(pData, steps, counts); + set_lastMotion(self, self->currSteps, self->currCounts); return 1; } else { if (self->debug) { @@ -840,7 +1011,7 @@ static int checkMotion(void *pData) { self->name, ratio_obs, ratio_exp, cmp); SICSLogWrite(msg, eError); } - set_lastMotion(pData, steps, counts); + set_lastMotion(self, self->currSteps, self->currCounts); return 1; } return 1; @@ -1190,6 +1361,11 @@ static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) { if (iRet == 0) break; set_lastMotion(self, self->currSteps, self->currCounts); + if (self->pMot == NULL) + self->pMot = FindMotor(pServ->pSics, self->name); + if (self->pMot) { + self->pMot->fTarget = self->pMot->fPosition = motPosit(self); + } snprintf(cmd, CMDLEN, "LV"); DMC_SendCmd(self, cmd, state_msg_callback); self->subState = 2; @@ -2194,7 +2370,7 @@ static int DMC2280GetPar(void *pData, char *name, /* XXX Maybe move this to a configuration parameter.*/ if(strcasecmp(name,HOME) == 0) { - *fValue = self->home; + *fValue = self->fHome; return 1; } if(strcasecmp(name,HARDLOWERLIM) == 0) { @@ -2295,6 +2471,19 @@ static int DMC2280GetPar(void *pData, char *name, return 0; } } + + 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) @@ -2328,7 +2517,7 @@ static int DMC2280GetPar(void *pData, char *name, * - 0 request failed * */ static int DMC2280SetPar(void *pData, SConnection *pCon, - char *name, float newValue){ + char *name, float newValue) { pDMC2280Driv self = NULL; char pError[ERRLEN]; char cmd[CMDLEN]; @@ -2351,7 +2540,7 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, SCWrite(pCon, pError, eError); return 1; } - self->home = newValue; + self->fHome = newValue; return 1; } } @@ -2559,8 +2748,20 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, return 1; } + if (self->posit_count > 0) { + int index; + if (strncasecmp(name, "posit_", 6) == 0 && isdigit(name[6])) { + index = strtol(&name[6], NULL, 10); + if (index < 1 || index > self->posit_count) + return 0; + self->posit_array[index - 1] = newValue; + return 1; + } + } + return 0; } + /** \brief List the motor parameters to the client. * \param self (r) provides access to the motor's data structure * \param *name (r) name of motor. @@ -2588,7 +2789,7 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){ pDMC2280Driv self = (pDMC2280Driv) pData; char buffer[BUFFLEN]; - snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->home); + snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->fHome); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed); SCWrite(pCon, buffer, eStatus); @@ -2634,6 +2835,17 @@ static void DMC2280List(void *pData, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); } } + if (self->posit_count > 0) { + int i; + snprintf(buffer, BUFFLEN, "%s.posit_count = %d\n", name, + self->posit_count); + 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]); + SCWrite(pCon, buffer, eStatus); + } + } snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX); SCWrite(pCon, buffer, eStatus); return; @@ -2689,6 +2901,11 @@ static void KillDMC2280(/*@only@*/void *pData){ AsyncUnitDestroy(self->asyncUnit); self->asyncUnit = NULL; } + if (self->posit_array) { + free(self->posit_array); + self->posit_array = NULL; + self->posit_count = 0; + } /* Not required as performed in caller * free(self); */ @@ -2793,9 +3010,9 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pNew->pMot = NULL; strcpy(pNew->name, motor); - pNew->home = 0.0; - pNew->fLower = 0.0;//(float)atof(argv[2]); - pNew->fUpper = 0.0;//(float)atof(argv[3]); + pNew->fHome = 0.0; + pNew->fLower = 0.0; + pNew->fUpper = 0.0; pNew->GetPosition = DMC2280GetPos; pNew->RunTo = DMC2280Run; pNew->GetStatus = DMC2280Status; @@ -2963,6 +3180,32 @@ 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; + 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->posit_array = malloc(pNew->posit_count * sizeof(*pNew->posit_array)); + 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->posit_array[i] = -1; + } + else { + pNew->posit_array[i] = strtol(pPtr, NULL, 10); + } + } + } + } } change_state(pNew, DMCState_Unknown); @@ -3130,10 +3373,107 @@ int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData, } else { self->trace = 0; - SCWrite(pCon, "TRACE OFF", eValue); + SCWrite(pCon, "TRACE OFF", eValue); } return 1; } + else if(strcasecmp("unit2count", argv[1]) == 0) { + char line[132]; + if (argc > 2) { + double target; + target = strtod(argv[2], NULL); + snprintf(line, 132, "%s.unit2count = %lld", + self->name, + unit2count(self, target)); + } + else + strcpy(line, "missing value"); + SCWrite(pCon, line, eValue); + return 1; + } + else if(strcasecmp("count2unit", argv[1]) == 0) { + char line[132]; + if (argc > 2) { + long long target; + target = strtoll(argv[2], NULL, 10); + snprintf(line, 132, "%s.count2unit = %f", + self->name, + count2unit(self, target)); + } + else + strcpy(line, "missing value"); + SCWrite(pCon, line, eValue); + return 1; + } + if (self->posit_count > 0) { + if(strcasecmp("posit2count", argv[1]) == 0) { + char line[132]; + if (argc > 2) { + double target; + target = strtod(argv[2], NULL); + snprintf(line, 132, "%s.posit2count = %lld", + self->name, + posit2count(self, target)); + } + else + strcpy(line, "missing value"); + SCWrite(pCon, line, eValue); + return 1; + } + else if(strcasecmp("count2posit", argv[1]) == 0) { + char line[132]; + if (argc > 2) { + long long target; + target = strtoll(argv[2], NULL, 10); + snprintf(line, 132, "%s.count2posit = %f", + self->name, + count2posit(self, target)); + } + else + strcpy(line, "missing value"); + SCWrite(pCon, line, eValue); + return 1; + } + else if(strcasecmp("unit2posit", argv[1]) == 0) { + char line[132]; + if (argc > 2) { + double target; + target = strtod(argv[2], NULL); + snprintf(line, 132, "%s.unit2posit = %f", + self->name, + unit2posit(self, target)); + } + else + strcpy(line, "missing value"); + SCWrite(pCon, line, eValue); + return 1; + } + else if(strcasecmp("posit2unit", argv[1]) == 0) { + char line[132]; + if (argc > 2) { + double target; + target = strtod(argv[2], NULL); + snprintf(line, 132, "%s.posit2unit = %f", + self->name, + posit2unit(self, target)); + } + else + strcpy(line, "missing value"); + SCWrite(pCon, line, eValue); + return 1; + } + else if(strcasecmp("posit", argv[1]) == 0) { + char line[132]; + int target = self->currCounts; + if (argc > 2) + target = strtol(argv[2], NULL, 10); + snprintf(line, 132, "%s.posit = %f", + self->name, + count2posit(self, target)); + SCWrite(pCon, line, eValue); + return 1; + } + } } return MotorAction(pCon, pSics, pData, argc, argv); }