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

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