linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Change stepsPerX and CountsPerX to float and revise PA calculation to avoid floting point limitation in GALIL

Browse Source 
r1841 | dcl | 2007-04-10 09:50:47 +1000 (Tue, 10 Apr 2007) | 2 lines
This commit is contained in:
Douglas Clowes
2007-04-10 09:50:47 +10:00
parent 72fd4d9ff5
commit a7a480c5d1
1 changed files with 66 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
site_ansto/motor_dmc2280.c
View File

@@ -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 */