linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Adapted indenation to new agreed upon system

Browse Source 
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
2009-02-13 09:01:03 +00:00
parent 6c7bb14fad
commit eb72d5c486
151 changed files with 38234 additions and 38208 deletions
Download Patch File Download Diff File Expand all files Collapse all files

285
swmotor2.c
View File

@ -29,285 +29,270 @@
#include "swmotor.h"
#include "swmotor.i"
typedef struct __MoDriv {
/* general motor driver interface
fields. REQUIRED!
*/
float fUpper; /* upper limit */
float fLower; /* lower limit */
char *name;
int (*GetPosition)(void *self,float *fPos);
int (*RunTo)(void *self, float fNewVal);
int (*GetStatus)(void *self);
void (*GetError)(void *self, int *iCode,
char *buffer, int iBufLen);
int (*TryAndFixIt)(void *self,int iError,
float fNew);
int (*Halt)(void *self);
int (*GetDriverPar)(void *self, char *name,
float *value);
int (*SetDriverPar)(void *self,SConnection *pCon,
char *name, float newValue);
void (*ListDriverPar)(void *self, char *motorName,
SConnection *pCon);
void (*KillPrivate)(void *self);
/* EL-734 specific fields */
prs232 controller;
int iMotor;
float lastValue;
int errorCode;
int oredMsr;
} EL734Driv, *pEL734Driv;
typedef struct __MoDriv {
/* general motor driver interface
fields. REQUIRED!
*/
float fUpper; /* upper limit */
float fLower; /* lower limit */
char *name;
int (*GetPosition) (void *self, float *fPos);
int (*RunTo) (void *self, float fNewVal);
int (*GetStatus) (void *self);
void (*GetError) (void *self, int *iCode, char *buffer, int iBufLen);
int (*TryAndFixIt) (void *self, int iError, float fNew);
int (*Halt) (void *self);
int (*GetDriverPar) (void *self, char *name, float *value);
int (*SetDriverPar) (void *self, SConnection * pCon,
char *name, float newValue);
void (*ListDriverPar) (void *self, char *motorName, SConnection * pCon);
void (*KillPrivate) (void *self);
/* EL-734 specific fields */
prs232 controller;
int iMotor;
float lastValue;
int errorCode;
int oredMsr;
} EL734Driv, *pEL734Driv;
/*========================================================================
We start of by implementing the interface functions for the various
interfaces this module has to implement.
==========================================================================*/
static int SWHalt(void *pData)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
assert(self);
return self->pMaster->pDrivInt->Halt(self->pMaster);
}
/*--------------------------------------------------------------------*/
static int SWCheckLimits(void *pData, float fVal, char *error, int iErrLen)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
assert(self);
return self->pMaster->pDrivInt->CheckLimits(self->pMaster,
fVal,error,iErrLen);
fVal, error, iErrLen);
}
/*--------------------------------------------------------------------*/
static int SWCheckStatus(void *pData, SConnection *pCon)
static int SWCheckStatus(void *pData, SConnection * pCon)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
assert(self);
return self->pMaster->pDrivInt->CheckStatus(self->pMaster,
pCon);
return self->pMaster->pDrivInt->CheckStatus(self->pMaster, pCon);
}
/*---------------------------------------------------------------------*/
static int SWSaveStatus(void *pData, char *name, FILE *fd)
static int SWSaveStatus(void *pData, char *name, FILE * fd)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
assert(self);
fprintf(fd,"%s savedValue = %f\n", name,
self->positions[self->myNumber]);
fprintf(fd, "%s savedValue = %f\n", name,
self->positions[self->myNumber]);
return 1;
}
/*-----------------------------------------------------------------------*/
static void *SWGetInterface(void *pData, int ID)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
assert(self);
if(ID == DRIVEID)
{
if (ID == DRIVEID) {
return self->pDriv;
}
return NULL;
}
/*----------------------------------------------------------------------*/
static float SWGetValue(void *pData, SConnection *pCon)
static float SWGetValue(void *pData, SConnection * pCon)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
float fVal;
assert(self);
/*
we are not selected: return stored data:
*/
if(self->myNumber != self->selectedMotor[0])
{
SCWrite(pCon,"WARNING: motor not active, returning stored value",
we are not selected: return stored data:
*/
if (self->myNumber != self->selectedMotor[0]) {
SCWrite(pCon, "WARNING: motor not active, returning stored value",
eWarning);
return self->positions[self->myNumber];
}
else
{
} else {
fVal = self->pMaster->pDrivInt->GetValue(self->pMaster, pCon);
self->positions[self->myNumber] = fVal;
return fVal;
}
}
/*-----------------------------------------------------------------------*/
static long SWSetValue(void *pData, SConnection *pCon, float fVal)
static long SWSetValue(void *pData, SConnection * pCon, float fVal)
{
pSWmot self = (pSWmot)pData;
pSWmot self = (pSWmot) pData;
char pCommand[256], pError[132];
int status;
EL734Driv *pElli;
EL734Driv *pElli;
Tcl_Interp *pTcl;
assert(self);
self->errCode = 1;
/*
first case: we are already selected
*/
if(self->myNumber == self->selectedMotor[0])
{
return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
}
else
{
first case: we are already selected
*/
if (self->myNumber == self->selectedMotor[0]) {
return self->pMaster->pDrivInt->SetValue(self->pMaster, pCon, fVal);
} else {
/*
second case: switch to the requested motor, do a reference run
and then execute Set command
*/
sprintf(pCommand,"Switching to motor number %d", self->myNumber);
SCWrite(pCon,pCommand,eWarning);
sprintf(pCommand,"%s %d",self->switchFunc, self->myNumber);
pTcl = (Tcl_Interp *)pServ->pSics->pTcl;
status = Tcl_Eval(pTcl,pCommand);
strncpy(pError,pTcl->result, 131);
if(status != TCL_OK || strstr(pError,"OK") == NULL)
{
sprintf(pCommand,"ERROR: %s while switching motor", pError);
SCWrite(pCon,pCommand, eError);
second case: switch to the requested motor, do a reference run
and then execute Set command
*/
sprintf(pCommand, "Switching to motor number %d", self->myNumber);
SCWrite(pCon, pCommand, eWarning);
sprintf(pCommand, "%s %d", self->switchFunc, self->myNumber);
pTcl = (Tcl_Interp *) pServ->pSics->pTcl;
status = Tcl_Eval(pTcl, pCommand);
strncpy(pError, pTcl->result, 131);
if (status != TCL_OK || strstr(pError, "OK") == NULL) {
sprintf(pCommand, "ERROR: %s while switching motor", pError);
SCWrite(pCon, pCommand, eError);
self->errCode = -1001;
return HWFault;
}
self->selectedMotor[0] = self->myNumber;
return self->pMaster->pDrivInt->SetValue(self->pMaster,pCon,fVal);
return self->pMaster->pDrivInt->SetValue(self->pMaster, pCon, fVal);
}
}
}
/*----------------------------------------------------------------------*/
static void KillSWFull(void *pData)
{
pSWmot self = (pSWmot)pData;
if(self == NULL)
pSWmot self = (pSWmot) pData;
if (self == NULL)
return;
if(self->pDriv)
if (self->pDriv)
free(self->pDriv);
if(self->pDes)
if (self->pDes)
DeleteDescriptor(self->pDes);
if(self->selectedMotor)
if (self->selectedMotor)
free(self->selectedMotor);
if(self->switchFunc)
if (self->switchFunc)
free(self->switchFunc);
free(self);
}
/*---------------------------------------------------------------------*/
static void KillSWHalf(void *pData)
{
pSWmot self = (pSWmot)pData;
if(self)
pSWmot self = (pSWmot) pData;
if (self)
free(self);
}
/*----------------------------------------------------------------------
Alright, now the interpreter functions follow
Usage:
MakeSWHPMotor master switchFunc slave1 slave2 slave3
-----------------------------------------------------------------------*/
int MakeSWHPMotor(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[])
int MakeSWHPMotor(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
pSWmot sw1, sw2, sw3;
char pBueffel[256];
int i, status;
/*
check that we have enough arguments
*/
if(argc < 6)
{
SCWrite(pCon,"ERROR: insufficient number of arguments to MakeSWMotor",
check that we have enough arguments
*/
if (argc < 6) {
SCWrite(pCon, "ERROR: insufficient number of arguments to MakeSWMotor",
eError);
return 0;
}
/*
allocate a new data structure
*/
sw1 = (pSWmot)malloc(sizeof(SWmot));
if(sw1 == NULL)
{
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
allocate a new data structure
*/
sw1 = (pSWmot) malloc(sizeof(SWmot));
if (sw1 == NULL) {
SCWrite(pCon, "ERROR: out of memory in MakeSWMotor", eError);
return 0;
}
memset(sw1,0,sizeof(SWmot));
memset(sw1, 0, sizeof(SWmot));
/*
fill it up with stuff
*/
fill it up with stuff
*/
sw1->pDes = CreateDescriptor("Sparbroetchen");
sw1->pDriv = CreateDrivableInterface();
sw1->selectedMotor = (int *)malloc(sizeof(int));
if(!sw1->pDes || ! sw1->pDriv || !sw1->selectedMotor)
{
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
sw1->selectedMotor = (int *) malloc(sizeof(int));
if (!sw1->pDes || !sw1->pDriv || !sw1->selectedMotor) {
SCWrite(pCon, "ERROR: out of memory in MakeSWMotor", eError);
return 0;
}
sw1->selectedMotor[0] = -1;
sw1->pMaster = FindMotor(pSics,argv[1]);
if(!sw1->pMaster)
{
sprintf(pBueffel,"ERROR: cannot find master motor %s", argv[1]);
SCWrite(pCon,pBueffel,eError);
sw1->pMaster = FindMotor(pSics, argv[1]);
if (!sw1->pMaster) {
sprintf(pBueffel, "ERROR: cannot find master motor %s", argv[1]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
}
sw1->switchFunc = strdup(argv[2]);
for(i = 0; i < 3; i++)
{
strcpy(sw1->slaves[i],argv[3+i]);
}
for (i = 0; i < 3; i++) {
strcpy(sw1->slaves[i], argv[3 + i]);
}
/*
initialize function pointers
*/
initialize function pointers
*/
sw1->pDes->SaveStatus = SWSaveStatus;
sw1->pDes->GetInterface = SWGetInterface;
sw1->pDriv->GetValue = SWGetValue;
sw1->pDriv->SetValue = SWSetValue;
sw1->pDriv->Halt = SWHalt;
sw1->pDriv->Halt = SWHalt;
sw1->pDriv->CheckStatus = SWCheckStatus;
sw1->pDriv->CheckLimits = SWCheckLimits;
/*
create clones of the new data structure of the other slaves
*/
sw2 = (pSWmot)malloc(sizeof(SWmot));
sw3 = (pSWmot)malloc(sizeof(SWmot));
if(!sw2 || !sw2)
{
SCWrite(pCon,"ERROR: out of memory in MakeSWMotor",eError);
create clones of the new data structure of the other slaves
*/
sw2 = (pSWmot) malloc(sizeof(SWmot));
sw3 = (pSWmot) malloc(sizeof(SWmot));
if (!sw2 || !sw2) {
SCWrite(pCon, "ERROR: out of memory in MakeSWMotor", eError);
return 0;
}
memcpy(sw2,sw1,sizeof(SWmot));
memcpy(sw2, sw1, sizeof(SWmot));
sw2->myNumber = 1;
memcpy(sw3,sw1,sizeof(SWmot));
memcpy(sw3, sw1, sizeof(SWmot));
sw3->myNumber = 2;
/*
install commands
*/
install commands
*/
status = AddCommand(pSics, argv[3], SWMotorAction, KillSWFull, sw1);
if(!status)
{
sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
SCWrite(pCon,pBueffel,eError);
if (!status) {
sprintf(pBueffel, "ERROR: command %s already exists!", argv[3]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
status = AddCommand(pSics, argv[4], SWMotorAction, KillSWHalf, sw2);
if(!status)
{
sprintf(pBueffel,"ERROR: command %s already exists!",argv[4]);
SCWrite(pCon,pBueffel,eError);
if (!status) {
sprintf(pBueffel, "ERROR: command %s already exists!", argv[4]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
status = AddCommand(pSics, argv[5], SWMotorAction, KillSWHalf, sw3);
if(!status)
{
sprintf(pBueffel,"ERROR: command %s already exists!",argv[3]);
SCWrite(pCon,pBueffel,eError);
if (!status) {
sprintf(pBueffel, "ERROR: command %s already exists!", argv[3]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
return 1;
}