PSI sics-cvs-psi-2006

lin2ang.c

@@ -8,6 +8,9 @@
   copyright: see copyright.h
 
   Mark Koennecke, February 2000
+
+  added zero point handling for the Jochen
+  Mark Koennecke, December 2005
   ---------------------------------------------------------------------------*/
 #include <stdlib.h>
 #include <math.h>
@@ -25,12 +28,13 @@
                              pIDrivable pDriv;
                              pMotor lin;
                              float length;
+                             float zero;
                 }Lin2Ang, *pLin2Ang;
 
 /*-------------------------- conversion routines -------------------------*/
   static float ang2x(pLin2Ang self, float fAngle)
   {
-     return self->length*sin(fAngle/RD);
+     return self->length*sin((fAngle+self->zero)/RD);
   } 
 /*-----------------------------------------------------------------------*/
   static float x2ang(pLin2Ang self, float fX)
@@ -40,7 +44,7 @@
     assert(self->length > 0.);
 
     dt = fX/self->length;
-    return RD*asin(dt);
+    return RD*asin(dt) - self->zero;
   }
 /*============== functions in the interface ============================*/
   static void *Lin2AngGetInterface(void *pData, int iID)
@@ -65,7 +69,8 @@
     if(!self)
        return 0;
 
-    fprintf(fd,"%s.length %f\n",name, self->length);
+    fprintf(fd,"%s length %f\n",name, self->length);
+    fprintf(fd,"%s zero %f\n",name, self->zero);
     return 1;
          
   }
@@ -99,9 +104,7 @@
 
       self = (pLin2Ang)pData;
       assert(self);
-      fX = self->lin->pDrivInt->GetValue(self->lin,pCon);    
-      MotorGetPar(self->lin,"softzero",&zero);
-      fX -= zero;
+      MotorGetSoftPosition(self->lin,pCon,&fX);
       return x2ang(self,fX);
   }
 /*------------------------------------------------------------------------*/
@@ -271,6 +274,35 @@
               return 1;         
            }
         }    
+/* zero point */
+        if(strcmp(argv[1],"zero") == 0)
+        {
+           if(argc >= 3)
+           {
+              iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
+              if(iRet != TCL_OK)
+              {
+                 SCWrite(pCon,"ERROR: zero parameter not recognised as number",
+                         eError);
+                 return 0;        
+              }
+              if(!SCMatchRights(pCon,usUser))
+              {
+                SCWrite(pCon,"ERROR: Insufficient privilege to change zero point",
+                        eError);
+                return 0;        
+              }
+              self->zero = dVal;
+              SCSendOK(pCon);
+              return 1;
+           }
+           else
+           {
+              sprintf(pBueffel,"%s.zero = %f",argv[0],self->zero);
+              SCWrite(pCon,pBueffel,eValue);
+              return 1;         
+           }
+        }    
 /* limits */
         if(strstr(argv[1],"lim") != NULL)
 	{