PSI sics-cvs-psi_pre-ansto
This commit is contained in:
Douglas Clowes
290
lin2ang.c
Normal file
290
lin2ang.c
Normal file
@@ -0,0 +1,290 @@
|
||||
/*--------------------------------------------------------------------------
|
||||
|
||||
L I N 2 A N G
|
||||
|
||||
A virtual motor device for driving an angle through a translation table.
|
||||
As of now special for TOPSI.
|
||||
|
||||
copyright: see copyright.h
|
||||
|
||||
Mark Koennecke, February 2000
|
||||
---------------------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
#include <tcl.h>
|
||||
#include "fortify.h"
|
||||
#include "sics.h"
|
||||
#include "lin2ang.h"
|
||||
|
||||
static const float RD = 57.2957795, pi = 3.1415926;
|
||||
|
||||
/* --------- our very own private data structure ------------------------*/
|
||||
typedef struct __LIN2ANG {
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDriv;
|
||||
pMotor lin;
|
||||
float length;
|
||||
}Lin2Ang, *pLin2Ang;
|
||||
|
||||
/*-------------------------- conversion routines -------------------------*/
|
||||
static float ang2x(pLin2Ang self, float fAngle)
|
||||
{
|
||||
return self->length*sin(fAngle/RD);
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static float x2ang(pLin2Ang self, float fX)
|
||||
{
|
||||
double dt;
|
||||
|
||||
assert(self->length > 0.);
|
||||
|
||||
dt = fX/self->length;
|
||||
return RD*asin(dt);
|
||||
}
|
||||
/*============== functions in the interface ============================*/
|
||||
static void *Lin2AngGetInterface(void *pData, int iID)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
|
||||
if(iID == DRIVEID)
|
||||
{
|
||||
return self->pDriv;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int Lin2AngSave(void *pData, char *name, FILE *fd)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
if(!self)
|
||||
return 0;
|
||||
|
||||
fprintf(fd,"%s.length %f\n",name, self->length);
|
||||
return 1;
|
||||
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int L2AHalt(void *pData)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
|
||||
return self->lin->pDrivInt->Halt(self->lin);
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int L2ALimits(void *pData, float fVal, char *error, int iErrlen)
|
||||
{
|
||||
float fX;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
|
||||
fX = ang2x(self,fVal);
|
||||
return self->lin->pDrivInt->CheckLimits(self->lin,fX,error,iErrlen);
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static float L2AGetValue(void *pData, SConnection *pCon)
|
||||
{
|
||||
float fX, zero = 0.;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
fX = self->lin->pDrivInt->GetValue(self->lin,pCon);
|
||||
MotorGetPar(self->lin,"softzero",&zero);
|
||||
fX -= zero;
|
||||
return x2ang(self,fX);
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int L2AStatus(void *pData, SConnection *pCon)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
return self->lin->pDrivInt->CheckStatus(self->lin,pCon);
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static long L2ASetValue(void *pData, SConnection *pCon, float fValue)
|
||||
{
|
||||
float fX;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
|
||||
fX = ang2x(self,fValue);
|
||||
return self->lin->pDrivInt->SetValue(self->lin,pCon,fX);
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static void KillL2A(void *pData)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
if(!self)
|
||||
return;
|
||||
|
||||
if(self->pDes)
|
||||
{
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
if(self->pDriv)
|
||||
{
|
||||
free(self->pDriv);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
/*-------------------------------------------------------------------
|
||||
Syntax: MakeLin2Ang name motor
|
||||
*/
|
||||
int MakeLin2Ang(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pLin2Ang pNew = NULL;
|
||||
char pBueffel[255];
|
||||
int iRet;
|
||||
|
||||
/* check number of arguments */
|
||||
if(argc < 3)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: Insufficient arguments to Lin2Arg",eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* allocate memory */
|
||||
pNew = (pLin2Ang)malloc(sizeof(Lin2Ang));
|
||||
if(!pNew)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeLin2Ang",eError);
|
||||
return 0;
|
||||
}
|
||||
memset(pNew,0,sizeof(Lin2Ang));
|
||||
pNew->pDes = CreateDescriptor("Lin2Ang");
|
||||
if(!pNew->pDes)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeLin2Ang",eError);
|
||||
free(pNew);
|
||||
return 0;
|
||||
}
|
||||
pNew->pDriv = CreateDrivableInterface();
|
||||
if(!pNew->pDriv)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: out of memory in MakeLin2Ang",eError);
|
||||
KillL2A(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check if we got a motor */
|
||||
pNew->lin = FindMotor(pSics,argv[2]);
|
||||
if(!pNew->lin)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: %s is no motor!",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
KillL2A(pNew);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* initialize the data structure */
|
||||
pNew->pDes->GetInterface = Lin2AngGetInterface;
|
||||
pNew->pDes->SaveStatus = Lin2AngSave;
|
||||
pNew->pDriv->Halt = L2AHalt;
|
||||
pNew->pDriv->CheckLimits = L2ALimits;
|
||||
pNew->pDriv->SetValue = L2ASetValue;
|
||||
pNew->pDriv->CheckStatus = L2AStatus;
|
||||
pNew->pDriv->GetValue = L2AGetValue;
|
||||
pNew->length = 80.;
|
||||
|
||||
/* install command */
|
||||
iRet = AddCommand(pSics, argv[1],Lin2AngAction,KillL2A,pNew);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,
|
||||
"ERROR: duplicate Lin2Ang command %s NOT created",
|
||||
argv[1]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
KillL2A(pNew);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
int Lin2AngAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
char pBueffel[255];
|
||||
float fVal, fLow, fHigh;
|
||||
double dVal;
|
||||
int iRet;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
assert(pCon);
|
||||
|
||||
/* without parameter: give value */
|
||||
if(argc < 2)
|
||||
{
|
||||
fVal = L2AGetValue(self,pCon);
|
||||
sprintf(pBueffel,"%s = %f",argv[0],fVal);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* interpret commands */
|
||||
strtolower(argv[1]);
|
||||
if(strcmp(argv[1],"length") == 0)
|
||||
{
|
||||
if(argc >= 3)
|
||||
{
|
||||
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: length parameter not recognised as number",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
SCWrite(pCon,"ERROR: Insufficient privilege to change length",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
self->length = dVal;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"%s.length = %f",argv[0],self->length);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
/* limits */
|
||||
if(strstr(argv[1],"lim") != NULL)
|
||||
{
|
||||
MotorGetPar(self->lin,"softupperlim",&fHigh);
|
||||
MotorGetPar(self->lin,"softlowerlim",&fLow);
|
||||
fHigh = x2ang(self,fHigh);
|
||||
fLow = x2ang(self,fLow);
|
||||
sprintf(pBueffel,"%s.limits: %f %f\n change through motor limits ",
|
||||
argv[0],fLow,fHigh);
|
||||
SCWrite(pCon,pBueffel,eValue);
|
||||
return 1;
|
||||
}
|
||||
sprintf(pBueffel,"ERROR: method %s not found!",argv[1]);
|
||||
SCWrite(pCon, pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user