- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
534
o2t.c
534
o2t.c
@ -49,282 +49,270 @@
|
||||
#include "fupa.h"
|
||||
#include "o2t.h"
|
||||
|
||||
typedef struct __SicsO2T {
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDrivInt;
|
||||
pDummy pOmega;
|
||||
pDummy pTheta;
|
||||
} SicsO2T;
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static void *GetO2TInterface(void *pData, int iID)
|
||||
{
|
||||
SicsO2T *self = NULL;
|
||||
|
||||
self = (SicsO2T *)pData;
|
||||
assert(self);
|
||||
if(iID == DRIVEID)
|
||||
{
|
||||
return self->pDrivInt;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int O2THalt(void *pData)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
|
||||
self = (pSicsO2T)pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt = self->pOmega->pDescriptor->GetInterface(self->pOmega,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
pDrivInt->Halt(self->pOmega);
|
||||
}
|
||||
pDrivInt = self->pTheta->pDescriptor->GetInterface(self->pTheta,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
pDrivInt->Halt(self->pTheta);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static int O2TCheckLimits(void *pData, float fVal, char *pError,
|
||||
int iErrLen)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
int iRet;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
|
||||
self = (pSicsO2T)pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt = self->pOmega->pDescriptor->GetInterface(self->pOmega,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
iRet = pDrivInt->CheckLimits(self->pOmega, fVal/2.,
|
||||
pError, iErrLen);
|
||||
if(!iRet)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
pDrivInt = self->pTheta->pDescriptor->GetInterface(self->pTheta,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
return pDrivInt->CheckLimits(self->pTheta, fVal,
|
||||
pError, iErrLen);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static long O2TSetValue(void *pData, SConnection *pCon, float fVal)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
int iRet;
|
||||
|
||||
self = (pSicsO2T)pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt = self->pOmega->pDescriptor->GetInterface(self->pOmega,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
iRet = pDrivInt->SetValue(self->pOmega,pCon, fVal/2.);
|
||||
if(iRet != OKOK)
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
pDrivInt = self->pTheta->pDescriptor->GetInterface(self->pTheta,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
iRet = pDrivInt->SetValue(self->pTheta,pCon, fVal);
|
||||
return iRet;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static int O2TCheckStatus(void *pData, SConnection *pCon)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
int iRet;
|
||||
|
||||
self = (pSicsO2T)pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt = self->pOmega->pDescriptor->GetInterface(self->pOmega,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
iRet = pDrivInt->CheckStatus(self->pOmega,pCon);
|
||||
if((iRet != OKOK) && (iRet != HWIdle) )
|
||||
{
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
pDrivInt = self->pTheta->pDescriptor->GetInterface(self->pTheta,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
iRet = pDrivInt->CheckStatus(self->pTheta,pCon);
|
||||
return iRet;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static float O2TGetValue(void *pData, SConnection *pCon)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
|
||||
self = (pSicsO2T)pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt = self->pTheta->pDescriptor->GetInterface(self->pTheta,DRIVEID);
|
||||
if(pDrivInt)
|
||||
{
|
||||
return pDrivInt->GetValue(self->pTheta,pCon);
|
||||
}
|
||||
return -9999.;
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
pSicsO2T MakeO2T(char *omega, char *theta, SicsInterp *pSics)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
CommandList *pCom = NULL;
|
||||
pIDrivable pDriv = NULL;
|
||||
pDummy pDum = NULL;
|
||||
|
||||
/* allocate memory */
|
||||
self = (pSicsO2T)malloc(sizeof(SicsO2T));
|
||||
if(!self)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
self->pDes = CreateDescriptor("Omega2Theta");
|
||||
if(!self->pDes)
|
||||
{
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* get and check drivabels */
|
||||
pCom = FindCommand(pSics,omega);
|
||||
if(pCom != NULL)
|
||||
{
|
||||
pDum = pCom->pData;
|
||||
} else
|
||||
{
|
||||
pDum = NULL;
|
||||
}
|
||||
if(GetDrivableInterface(pDum) != NULL){
|
||||
self->pOmega = pDum;
|
||||
}
|
||||
pCom = FindCommand(pSics,theta);
|
||||
if(pCom != NULL)
|
||||
{
|
||||
pDum = pCom->pData;
|
||||
}
|
||||
else
|
||||
{
|
||||
pDum = NULL;
|
||||
}
|
||||
if(GetDrivableInterface(pDum) != NULL){
|
||||
self->pTheta = pDum;
|
||||
}
|
||||
if( (self->pOmega == NULL) || (self->pTheta == NULL) )
|
||||
{
|
||||
DeleteDescriptor(self->pDes);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* initialize Descriptor */
|
||||
self->pDes->GetInterface = GetO2TInterface;
|
||||
|
||||
|
||||
/* initialise drivable interface */
|
||||
self->pDrivInt = CreateDrivableInterface();
|
||||
if(!self->pDrivInt)
|
||||
{
|
||||
DeleteDescriptor(self->pDes);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
self->pDrivInt->Halt = O2THalt;
|
||||
self->pDrivInt->CheckLimits = O2TCheckLimits;
|
||||
self->pDrivInt->SetValue = O2TSetValue;
|
||||
self->pDrivInt->CheckStatus = O2TCheckStatus;
|
||||
self->pDrivInt->GetValue = O2TGetValue;
|
||||
|
||||
return self;
|
||||
}
|
||||
/*---------------------------------------------------------------------------*/
|
||||
void DeleteO2T(void *pData)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
|
||||
self = (pSicsO2T)pData;
|
||||
assert(self);
|
||||
typedef struct __SicsO2T {
|
||||
pObjectDescriptor pDes;
|
||||
pIDrivable pDrivInt;
|
||||
pDummy pOmega;
|
||||
pDummy pTheta;
|
||||
} SicsO2T;
|
||||
|
||||
if(self->pDrivInt)
|
||||
{
|
||||
free(self->pDrivInt);
|
||||
}
|
||||
if(self->pDes)
|
||||
{
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static int DummyO2T(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
SCWrite(pCon,"WARNING: O2T does not understand any commnds",eWarning);
|
||||
return 1;
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static void *GetO2TInterface(void *pData, int iID)
|
||||
{
|
||||
SicsO2T *self = NULL;
|
||||
|
||||
self = (SicsO2T *) pData;
|
||||
assert(self);
|
||||
if (iID == DRIVEID) {
|
||||
return self->pDrivInt;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
static int O2THalt(void *pData)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
|
||||
self = (pSicsO2T) pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt =
|
||||
self->pOmega->pDescriptor->GetInterface(self->pOmega, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
pDrivInt->Halt(self->pOmega);
|
||||
}
|
||||
pDrivInt =
|
||||
self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
pDrivInt->Halt(self->pTheta);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static int O2TCheckLimits(void *pData, float fVal, char *pError,
|
||||
int iErrLen)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
int iRet;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
|
||||
self = (pSicsO2T) pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt =
|
||||
self->pOmega->pDescriptor->GetInterface(self->pOmega, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
iRet = pDrivInt->CheckLimits(self->pOmega, fVal / 2., pError, iErrLen);
|
||||
if (!iRet) {
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
pDrivInt =
|
||||
self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
return pDrivInt->CheckLimits(self->pTheta, fVal, pError, iErrLen);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static long O2TSetValue(void *pData, SConnection * pCon, float fVal)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
int iRet;
|
||||
|
||||
self = (pSicsO2T) pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt =
|
||||
self->pOmega->pDescriptor->GetInterface(self->pOmega, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
iRet = pDrivInt->SetValue(self->pOmega, pCon, fVal / 2.);
|
||||
if (iRet != OKOK) {
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
pDrivInt =
|
||||
self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
iRet = pDrivInt->SetValue(self->pTheta, pCon, fVal);
|
||||
return iRet;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static int O2TCheckStatus(void *pData, SConnection * pCon)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
int iRet;
|
||||
|
||||
self = (pSicsO2T) pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt =
|
||||
self->pOmega->pDescriptor->GetInterface(self->pOmega, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
iRet = pDrivInt->CheckStatus(self->pOmega, pCon);
|
||||
if ((iRet != OKOK) && (iRet != HWIdle)) {
|
||||
return iRet;
|
||||
}
|
||||
}
|
||||
pDrivInt =
|
||||
self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
iRet = pDrivInt->CheckStatus(self->pTheta, pCon);
|
||||
return iRet;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static float O2TGetValue(void *pData, SConnection * pCon)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
pIDrivable pDrivInt = NULL;
|
||||
|
||||
self = (pSicsO2T) pData;
|
||||
assert(self);
|
||||
|
||||
pDrivInt =
|
||||
self->pTheta->pDescriptor->GetInterface(self->pTheta, DRIVEID);
|
||||
if (pDrivInt) {
|
||||
return pDrivInt->GetValue(self->pTheta, pCon);
|
||||
}
|
||||
return -9999.;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
pSicsO2T MakeO2T(char *omega, char *theta, SicsInterp * pSics)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
CommandList *pCom = NULL;
|
||||
pIDrivable pDriv = NULL;
|
||||
pDummy pDum = NULL;
|
||||
|
||||
/* allocate memory */
|
||||
self = (pSicsO2T) malloc(sizeof(SicsO2T));
|
||||
if (!self) {
|
||||
return NULL;
|
||||
}
|
||||
self->pDes = CreateDescriptor("Omega2Theta");
|
||||
if (!self->pDes) {
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* get and check drivabels */
|
||||
pCom = FindCommand(pSics, omega);
|
||||
if (pCom != NULL) {
|
||||
pDum = pCom->pData;
|
||||
} else {
|
||||
pDum = NULL;
|
||||
}
|
||||
if (GetDrivableInterface(pDum) != NULL) {
|
||||
self->pOmega = pDum;
|
||||
}
|
||||
pCom = FindCommand(pSics, theta);
|
||||
if (pCom != NULL) {
|
||||
pDum = pCom->pData;
|
||||
} else {
|
||||
pDum = NULL;
|
||||
}
|
||||
if (GetDrivableInterface(pDum) != NULL) {
|
||||
self->pTheta = pDum;
|
||||
}
|
||||
if ((self->pOmega == NULL) || (self->pTheta == NULL)) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* initialize Descriptor */
|
||||
self->pDes->GetInterface = GetO2TInterface;
|
||||
|
||||
|
||||
/* initialise drivable interface */
|
||||
self->pDrivInt = CreateDrivableInterface();
|
||||
if (!self->pDrivInt) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
self->pDrivInt->Halt = O2THalt;
|
||||
self->pDrivInt->CheckLimits = O2TCheckLimits;
|
||||
self->pDrivInt->SetValue = O2TSetValue;
|
||||
self->pDrivInt->CheckStatus = O2TCheckStatus;
|
||||
self->pDrivInt->GetValue = O2TGetValue;
|
||||
|
||||
return self;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
void DeleteO2T(void *pData)
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
|
||||
self = (pSicsO2T) pData;
|
||||
assert(self);
|
||||
|
||||
if (self->pDrivInt) {
|
||||
free(self->pDrivInt);
|
||||
}
|
||||
if (self->pDes) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------------------*/
|
||||
static int DummyO2T(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
SCWrite(pCon, "WARNING: O2T does not understand any commnds", eWarning);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------
|
||||
initialisation command.
|
||||
Syntax: CreateO2T name omegamotor thetamotor
|
||||
*/
|
||||
int CreateO2T(SConnection *pCon, SicsInterp *pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
char pBueffel[256];
|
||||
int iRet;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
|
||||
if(argc < 4)
|
||||
{
|
||||
SCWrite(pCon,"ERROR: Insufficienet number of argumnets to CreateO2T",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* make O2T */
|
||||
self = MakeO2T(argv[2],argv[3],pSics);
|
||||
if(!self)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: no Memory or %s %s are no valid motor names",
|
||||
argv[2], argv[3]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* install command */
|
||||
iRet = AddCommand(pSics,argv[1],DummyO2T,DeleteO2T,self);
|
||||
if(!iRet)
|
||||
{
|
||||
sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[2]);
|
||||
SCWrite(pCon,pBueffel,eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
*/
|
||||
int CreateO2T(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pSicsO2T self = NULL;
|
||||
char pBueffel[256];
|
||||
int iRet;
|
||||
|
||||
assert(pCon);
|
||||
assert(pSics);
|
||||
|
||||
if (argc < 4) {
|
||||
SCWrite(pCon, "ERROR: Insufficienet number of argumnets to CreateO2T",
|
||||
eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* make O2T */
|
||||
self = MakeO2T(argv[2], argv[3], pSics);
|
||||
if (!self) {
|
||||
sprintf(pBueffel, "ERROR: no Memory or %s %s are no valid motor names",
|
||||
argv[2], argv[3]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* install command */
|
||||
iRet = AddCommand(pSics, argv[1], DummyO2T, DeleteO2T, self);
|
||||
if (!iRet) {
|
||||
sprintf(pBueffel, "ERROR: duplicate command %s not created", argv[2]);
|
||||
SCWrite(pCon, pBueffel, eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user