diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 9dd69ca0..d885dae7 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -105,6 +105,9 @@ typedef struct __MoDriv { float lastCounts; struct timeval time_lastPos_set; /**< Time when lastPosition was set */ float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */ + float blockage_thresh; /**< motion threshold for blockage checking */ + float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */ + int blockage_fail; /**< flag =1 if we should fail the motor */ int has_airpads; /**< Flag = 1 if there is are airpads for this motor */ float fTarget; int settle; @@ -670,8 +673,7 @@ static int DMC2280Run(void *pData,float fValue){ * - 0 MOTOR BLOCKED, no significant change in position detected. */ static int checkMotion(void *pData) { -#if 1 - float precision, steps, counts, ratio_obs, ratio_exp; + float precision, steps, counts, ratio_obs, ratio_exp, ratio_cmp; long int usec_TimeDiff; struct timeval now; @@ -690,14 +692,18 @@ static int checkMotion(void *pData) { MotorGetPar(self->pMot,"precision",&precision); if (FAILURE == readMotion(self, &steps, &counts)) return 0; - /* If not stepping, thern not blocked */ - if (steps == self->lastSteps) + /* If not stepping, then not blocked */ + if (fabs(steps - self->lastSteps) < self->blockage_thresh * self->cntsPerX) { + /* just update the timestamp */ + set_lastMotion(pData, self->lastSteps, self->lastCounts); return 1; + } /* calculate observed and expected steps per count ratios */ ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts); ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX; + ratio_cmp = ratio_obs / ratio_exp; /* less than half or more than double is trouble */ - if (ratio_obs / ratio_exp < 0.5 || ratio_obs / ratio_exp > 2.0) { + if (ratio_cmp > self->blockage_ratio || (1.0 / ratio_cmp) > self->blockage_ratio) { char msg[132]; snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f", ratio_obs, ratio_exp); @@ -706,17 +712,14 @@ static int checkMotion(void *pData) { steps, self->lastSteps, counts, self->lastCounts, self->stepsPerX, self->cntsPerX); SICSLogWrite(msg, eError); -#if 1 + if (self->blockage_fail) + return 0; set_lastMotion(pData, steps, counts); return 1; -#else - return 0; -#endif } else { set_lastMotion(pData, steps, counts); return 1; } -#endif return 1; } @@ -1157,6 +1160,18 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->blockage_ckInterval; return 1; } + if(strcmp(name,"blockage_thresh") == 0) { + *fValue = self->blockage_thresh; + return 1; + } + if(strcmp(name,"blockage_ratio") == 0) { + *fValue = self->blockage_ratio; + return 1; + } + if(strcmp(name,"blockage_fail") == 0) { + *fValue = self->blockage_fail; + return 1; + } if (self->abs_endcoder != 0) { if (strcmp(name,"absenc") == 0) { if (readAbsEnc(self, fValue) == SUCCESS) @@ -1271,6 +1286,39 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } } + /* Set interval between blocked motor checks, + * managers only */ + if(strcmp(name,"blockage_thresh") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->blockage_thresh = newValue; + return 1; + } + } + + /* Set interval between blocked motor checks, + * managers only */ + if(strcmp(name,"blockage_ratio") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->blockage_ratio = newValue; + return 1; + } + } + + /* Set interval between blocked motor checks, + * managers only */ + if(strcmp(name,"blockage_fail") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->blockage_fail = newValue; + return 1; + } + } + /* Set speed */ if(strcmp(name,SPEED) == 0) { if ((0.0 - newValue) > FLT_EPSILON) { @@ -1362,6 +1410,14 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, ((pDMC2280Driv)self)->settle); SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, ((pDMC2280Driv)self)->blockage_ckInterval); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, ((pDMC2280Driv)self)->blockage_thresh); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, ((pDMC2280Driv)self)->blockage_ratio); + SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, ((pDMC2280Driv)self)->blockage_fail); + SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, ((pDMC2280Driv)self)->has_airpads); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, ((pDMC2280Driv)self)->abs_endcoder); @@ -1522,6 +1578,9 @@ static void KillDMC2280(/*@only@*/void *pData){ pNew->ListDriverPar = DMC2280List; pNew->KillPrivate = KillDMC2280; pNew->blockage_ckInterval = 0.5; + pNew->blockage_thresh = 0.5; + pNew->blockage_ratio = 2.0; + pNew->blockage_fail = 0; /* PARAMETERS: Fetch parameter values */ if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {