- Added Sycamore protocol and command context to SICS
- Added sinfo to SICS - Added driver for TCP/IP Astrium velocity selector - Added driver for TCP/IP Astrium chopper controller SKIPPED: psi/amor2t.c psi/amorstat.c psi/dornier2.c psi/ecb.c psi/el734hp.c psi/fowrite.c psi/libpsi.a psi/make_gen psi/nextrics.c psi/pardef.c psi/pimotor.c psi/pipiezo.c psi/polterwrite.c psi/psi.c psi/scontroller.c psi/serial.c psi/tasinit.c psi/tasscan.c psi/tcpdocho.c psi/tcpdornier.c psi/tricssupport.c psi/velodornier.c
This commit is contained in:
44
lin2ang.c
44
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)
|
||||
{
|
||||
|
Reference in New Issue
Block a user