1.) Modified macro system as to use only Sicsunknown for resolving unknown
Tcl commands. Removed the broken obTcl object system and replaced it by
the object.tcl system from sntl. Redid the scan command with this. The
end of this is that SICS is now independent of the tcl version and
works with tcl 8.0 thus giving a factor of up to 10 in script execution
speed.
2.) Added driving an angle through a translation table (object lin2ang)
This commit is contained in:
cvs
273
lin2ang.c
Normal file
273
lin2ang.c
Normal file
@ -0,0 +1,273 @@
|
||||
/*--------------------------------------------------------------------------
|
||||
|
||||
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 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;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
fX = self->lin->pDrivInt->GetValue(self->lin,pCon);
|
||||
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->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