- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
509
lin2ang.c
509
lin2ang.c
@ -20,303 +20,290 @@
|
||||
#include "sics.h"
|
||||
#include "lin2ang.h"
|
||||
|
||||
static const float RD = 57.2957795, pi = 3.1415926;
|
||||
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;
|
||||
float zero;
|
||||
}Lin2Ang, *pLin2Ang;
|
||||
typedef struct __LIN2ANG {
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDriv;
|
||||
pMotor lin;
|
||||
float length;
|
||||
float zero;
|
||||
} Lin2Ang, *pLin2Ang;
|
||||
|
||||
/*-------------------------- conversion routines -------------------------*/
|
||||
static float ang2x(pLin2Ang self, float fAngle)
|
||||
{
|
||||
return self->length*tan((fAngle+self->zero)/RD);
|
||||
}
|
||||
static float ang2x(pLin2Ang self, float fAngle)
|
||||
{
|
||||
return self->length * tan((fAngle + self->zero) / RD);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static float x2ang(pLin2Ang self, float fX)
|
||||
{
|
||||
double dt;
|
||||
static float x2ang(pLin2Ang self, float fX)
|
||||
{
|
||||
double dt;
|
||||
|
||||
assert(self->length > 0.);
|
||||
assert(self->length > 0.);
|
||||
|
||||
dt = fX / self->length;
|
||||
return RD * atan(dt) - self->zero;
|
||||
}
|
||||
|
||||
dt = fX/self->length;
|
||||
return RD*atan(dt) - self->zero;
|
||||
}
|
||||
/*============== functions in the interface ============================*/
|
||||
static void *Lin2AngGetInterface(void *pData, int iID)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
static void *Lin2AngGetInterface(void *pData, int iID)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
if(iID == DRIVEID)
|
||||
{
|
||||
return self->pDriv;
|
||||
}
|
||||
return 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;
|
||||
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);
|
||||
fprintf(fd, "%s softzero %f\n", name, self->zero);
|
||||
return 1;
|
||||
|
||||
}
|
||||
|
||||
fprintf(fd,"%s length %f\n",name, self->length);
|
||||
fprintf(fd,"%s softzero %f\n",name, self->zero);
|
||||
return 1;
|
||||
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int L2AHalt(void *pData)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
static int L2AHalt(void *pData)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang) pData;
|
||||
assert(self);
|
||||
|
||||
return self->lin->pDrivInt->Halt(self->lin);
|
||||
}
|
||||
|
||||
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;
|
||||
static int L2ALimits(void *pData, float fVal, char *error, int iErrlen)
|
||||
{
|
||||
float fX;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
self = (pLin2Ang) pData;
|
||||
assert(self);
|
||||
|
||||
fX = ang2x(self, fVal);
|
||||
return self->lin->pDrivInt->CheckLimits(self->lin, fX, error, iErrlen);
|
||||
}
|
||||
|
||||
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;
|
||||
static float L2AGetValue(void *pData, SConnection * pCon)
|
||||
{
|
||||
float fX, zero = 0.;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang) pData;
|
||||
assert(self);
|
||||
MotorGetSoftPosition(self->lin, pCon, &fX);
|
||||
return x2ang(self, fX);
|
||||
}
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
MotorGetSoftPosition(self->lin,pCon,&fX);
|
||||
return x2ang(self,fX);
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int L2AStatus(void *pData, SConnection *pCon)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
static int L2AStatus(void *pData, SConnection * pCon)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang) pData;
|
||||
assert(self);
|
||||
return self->lin->pDrivInt->CheckStatus(self->lin, pCon);
|
||||
}
|
||||
|
||||
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;
|
||||
static long L2ASetValue(void *pData, SConnection * pCon, float fValue)
|
||||
{
|
||||
float fX;
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
self = (pLin2Ang) pData;
|
||||
assert(self);
|
||||
|
||||
fX = ang2x(self, fValue);
|
||||
return self->lin->pDrivInt->SetValue(self->lin, pCon, fX);
|
||||
}
|
||||
|
||||
fX = ang2x(self,fValue);
|
||||
return self->lin->pDrivInt->SetValue(self->lin,pCon,fX);
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static void KillL2A(void *pData)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
static void KillL2A(void *pData)
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
if(!self)
|
||||
return;
|
||||
self = (pLin2Ang) pData;
|
||||
if (!self)
|
||||
return;
|
||||
|
||||
if(self->pDes)
|
||||
{
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
if(self->pDriv)
|
||||
{
|
||||
free(self->pDriv);
|
||||
}
|
||||
free(self);
|
||||
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;
|
||||
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;
|
||||
/* 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 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;
|
||||
{
|
||||
pLin2Ang self = NULL;
|
||||
char pBueffel[255];
|
||||
float fVal, fLow, fHigh;
|
||||
double dVal;
|
||||
int iRet;
|
||||
|
||||
self = (pLin2Ang)pData;
|
||||
assert(self);
|
||||
assert(pCon);
|
||||
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;
|
||||
}
|
||||
}
|
||||
/* zero point */
|
||||
if(strcmp(argv[1],"softzero") == 0)
|
||||
{
|
||||
if(argc >= 3)
|
||||
{
|
||||
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
|
||||
if(iRet != TCL_OK)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: softzero parameter not recognised as number",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
if(!SCMatchRights(pCon,usUser))
|
||||
{
|
||||
SCWrite(pCon,"ERROR: Insufficient privilege to change softzero point",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
self->zero = dVal;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(pBueffel,"%s.softzero = %f",argv[0],self->zero);
|
||||
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;
|
||||
/* 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;
|
||||
}
|
||||
}
|
||||
/* zero point */
|
||||
if (strcmp(argv[1], "softzero") == 0) {
|
||||
if (argc >= 3) {
|
||||
iRet = Tcl_GetDouble(pSics->pTcl, argv[2], &dVal);
|
||||
if (iRet != TCL_OK) {
|
||||
SCWrite(pCon, "ERROR: softzero parameter not recognised as number",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: Insufficient privilege to change softzero point",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
self->zero = dVal;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else {
|
||||
sprintf(pBueffel, "%s.softzero = %f", argv[0], self->zero);
|
||||
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