diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 6019a832..96ff607b 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -100,13 +100,14 @@ typedef struct __MoDriv { 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; - struct timeval time_settle_done; - int airpad_state; - int airpad_counter; - pNWTimer airpad_timer; - pNWTimer motor_timer; + float backlash_offset; /**< signed offset to drive from */ + float fTarget; /**< target passed from SICS to timer callback */ + int settle; /**< motor settling time in seconds */ + struct timeval time_settle_done; /**< time when settling will be over */ + int airpad_state; /**< state of the airpads finite state machine */ + int airpad_counter; /**< airpad timer retry counter */ + pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */ + pNWTimer motor_timer; /**< motor off timer */ int debug; } DMC2280Driv, *pDMC2280Driv; @@ -711,6 +712,17 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ snprintf(BGx, CMDLEN, "BG%c", axis); target = fValue - self->home; + if (self->backlash_offset > FLT_EPSILON && target < self->lastPosition) { + target += self->backlash_offset; + if (target > self->fUpper) + target = self->fUpper; + } + if (self->backlash_offset < -FLT_EPSILON && target > self->lastPosition) { + target += self->backlash_offset; + if (target < self->fLower) + target = self->fLower; + } + if (1 == self->abs_endcoder) { absEncHome = self->absEncHome; cntsPerX = self->cntsPerX; @@ -762,7 +774,7 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ else break; snprintf(absPosCmd, CMDLEN, - "PA%c=(((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c", + "PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c", axis, absEncHome, axis, @@ -956,12 +968,11 @@ static int DMC2280Run(void *pData,float fValue){ * Note: this will read the current position which will block */ do { -#if 1 - self->time_lastPos_set.tv_sec = 0; -#else float currPos; DMC2280GetPos(pData, &currPos); - set_lastPos(pData, currPos); + self->lastPosition = currPos; +#if 1 + self->time_lastPos_set.tv_sec = 0; #endif } while (0); @@ -1503,6 +1514,10 @@ static int DMC2280GetPar(void *pData, char *name, *fValue = self->blockage_fail; return 1; } + if(strcasecmp(name,"backlash_offset") == 0) { + *fValue = self->backlash_offset; + return 1; + } if (self->abs_endcoder != 0) { if (strcasecmp(name,"absenc") == 0) { if (readAbsEnc(self, fValue) == SUCCESS) @@ -1678,6 +1693,17 @@ static int DMC2280SetPar(void *pData, SConnection *pCon, } } + /* Set backlash offset, + * managers only */ + if(strcasecmp(name,"backlash_offset") == 0) { + if(!SCMatchRights(pCon,usMugger)) + return 1; + else { + self->backlash_offset = newValue; + return 1; + } + } + /* Invoke Home Run routine in controller, * managers only */ if(self->abs_endcoder == 0 && strcasecmp(name,"homerun") == 0) { @@ -1796,6 +1822,8 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){ 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.Backlash_offset = %d\n", name, ((pDMC2280Driv)self)->backlash_offset); + 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); @@ -1943,6 +1971,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { pNew->blockage_thresh = 0.5; pNew->blockage_ratio = 2.0; pNew->blockage_fail = 0; + pNew->backlash_offset = 0.0; /* PARAMETERS: Fetch parameter values */ if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) { @@ -2019,6 +2048,13 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { sscanf(pPtr,"%d",&(pNew->settle)); } + /* BACKLASH: this motor need airpads */ + if ((pPtr=getParam(pCon, interp, params,"backlash_offset",_OPTIONAL)) == NULL) + pNew->backlash_offset=0.0; + else { + sscanf(pPtr,"%f",&(pNew->backlash_offset)); + } + /* AIRPADS: this motor need airpads */ if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL) pNew->has_airpads=0; @@ -2036,7 +2072,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { else sscanf(pPtr,"%d",&(pNew->absEncHome)); if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL) - pNew->cntsPerX=1; + pNew->cntsPerX=1.0; else sscanf(pPtr,"%f",&(pNew->cntsPerX)); }