From a7a480c5d1a8c2109e04e604fc759aad11dd29a8 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Tue, 10 Apr 2007 09:50:47 +1000 Subject: [PATCH] Change stepsPerX and CountsPerX to float and revise PA calculation to avoid floting point limitation in GALIL r1841 | dcl | 2007-04-10 09:50:47 +1000 (Tue, 10 Apr 2007) | 2 lines --- site_ansto/motor_dmc2280.c | 77 ++++++++++++++++++++++++++++++++------ 1 file changed, 66 insertions(+), 11 deletions(-) diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index b3725444..d68db4b3 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -86,10 +86,10 @@ typedef struct __MoDriv { float home; /**< home position for axis, default=0 */ int motorHome; /**< motor home position in steps */ int noPowerSave; /**< Flag = 1 to leave motors on after a move */ - int stepsPerX; /**< steps per physical unit */ + float stepsPerX; /**< steps per physical unit */ int abs_endcoder; /**< Flag = 1 if there is an abs enc */ int absEncHome; /**< Home position in counts for abs enc */ - int cntsPerX; /**< absolute encoder counts per physical unit */ + float cntsPerX; /**< absolute encoder counts per physical unit */ int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */ float lastPosition; /**< Position at last position check */ float lastSteps; @@ -193,7 +193,7 @@ struct __command { */ static int motSpeed(pDMC2280Driv self, float axisSpeed) { int speed; - speed = abs((int)(axisSpeed * self->stepsPerX + 0.5)); + speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5); return speed; } @@ -205,7 +205,7 @@ static int motSpeed(pDMC2280Driv self, float axisSpeed) { */ static int motAccel(pDMC2280Driv self, float axisAccel) { int accel; - accel = abs((int)(axisAccel * self->stepsPerX + 0.5)); + accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5); return accel; } @@ -217,7 +217,7 @@ static int motAccel(pDMC2280Driv self, float axisAccel) { */ static int motDecel(pDMC2280Driv self, float axisDecel) { int decel; - decel = abs((int)(axisDecel * self->stepsPerX + 0.5)); + decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5); return decel; } @@ -700,7 +700,8 @@ static int DMC2280GetPos(void *pData, float *fPos){ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ char axis; char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN]; - int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn; + int absEncHome, motorHome, newAbsPosn; + float stepsPerX, cntsPerX; float target; axis=self->axisLabel; @@ -713,9 +714,10 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ if (1 == self->abs_endcoder) { absEncHome = self->absEncHome; cntsPerX = self->cntsPerX; +#if 0 /* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */ snprintf(absPosCmd, CMDLEN, - "PA%c=(((%d-_TP%c)/%d)+%f)*%d + _TD%c", + "PA%c=(((%d-_TP%c)/%f)+%f)*%f + _TD%c", axis, absEncHome, axis, @@ -723,6 +725,53 @@ static int DMC2280RunCommon(pDMC2280Driv self,float fValue){ target, stepsPerX, axis); +#else + /* PAZ=((absEncHome-_TPZ) + (cntsPerX * target)) * stepsPerX / cntsPerX + _TDZ */ + char s_cnts[20]; + char s_trgt[20]; + char s_stps[20]; + int i; + snprintf(s_cnts, sizeof(s_cnts), "%.4f", cntsPerX); + for (i = strlen(s_cnts); i > 0; --i) + if (s_cnts[i-1] == '.') { + s_cnts[i-1] = '\0'; + break; + } + else if (s_cnts[i-1] == '0') + s_cnts[i-1] = '\0'; + else + break; + snprintf(s_trgt, sizeof(s_trgt), "%.4f", target); + for (i = strlen(s_trgt); i > 0; --i) + if (s_trgt[i-1] == '.') { + s_trgt[i-1] = '\0'; + break; + } + else if (s_trgt[i-1] == '0') + s_trgt[i-1] = '\0'; + else + break; + snprintf(s_stps, sizeof(s_stps), "%.4f", stepsPerX); + for (i = strlen(s_stps); i > 0; --i) + if (s_stps[i-1] == '.') { + s_stps[i-1] = '\0'; + break; + } + else if (s_stps[i-1] == '0') + s_stps[i-1] = '\0'; + else + break; + snprintf(absPosCmd, CMDLEN, + "PA%c=(((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c", + axis, + absEncHome, + axis, + s_cnts, + s_trgt, + s_stps, + s_cnts, + axis); +#endif #ifdef BACKLASHFIX do { char cmd[CMDLEN]; @@ -985,7 +1034,7 @@ static int checkMotion(void *pData) { snprintf(msg, sizeof(msg), "Motion check fail: obs=%f, exp=%f", ratio_obs, ratio_exp); SICSLogWrite(msg, eError); - snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%d/%d", + snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%f/%f", steps, self->lastSteps, counts, self->lastCounts, self->stepsPerX, self->cntsPerX); SICSLogWrite(msg, eError); @@ -1458,6 +1507,10 @@ static int DMC2280GetPar(void *pData, char *name, else return 0; } + if (strcasecmp(name,"absenchome") == 0) { + *fValue = self->absEncHome; + return 1; + } } else { if (strcasecmp(name,"homerun") == 0) { @@ -1708,6 +1761,8 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){ SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, ((pDMC2280Driv)self)->axisLabel); SCWrite(pCon, buffer, eStatus); + snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, ((pDMC2280Driv)self)->stepsPerX); + SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, ((pDMC2280Driv)self)->home); SCWrite(pCon, buffer, eStatus); snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, ((pDMC2280Driv)self)->units); @@ -1745,7 +1800,7 @@ static void DMC2280List(void *self, char *name, SConnection *pCon){ if (((pDMC2280Driv)self)->abs_endcoder) { snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, ((pDMC2280Driv)self)->absEncHome); SCWrite(pCon, buffer, eStatus); - snprintf(buffer, BUFFLEN, "%s.cntsPerX = %d\n", name, ((pDMC2280Driv)self)->cntsPerX); + snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, ((pDMC2280Driv)self)->cntsPerX); SCWrite(pCon, buffer, eStatus); } return; @@ -1933,7 +1988,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { KillDMC2280(pNew); return NULL; } - sscanf(pPtr,"%d",&(pNew->stepsPerX)); + sscanf(pPtr,"%f",&(pNew->stepsPerX)); if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL) pNew->motorHome=0; else @@ -1980,7 +2035,7 @@ MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) { if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL) pNew->cntsPerX=1; else - sscanf(pPtr,"%d",&(pNew->cntsPerX)); + sscanf(pPtr,"%f",&(pNew->cntsPerX)); } /* Set speed */