156 lines
3.9 KiB
C
156 lines
3.9 KiB
C
/*-------------------------------------------------------------------------
|
|
R e g M o t
|
|
|
|
This is a helper module for the Anti Collider. It handles all the
|
|
stuff necessary for dealing with a single motor. For more
|
|
information see the file anticollider.tex.
|
|
|
|
copyright: see file copyright
|
|
|
|
Mark Koennecke, August 2002
|
|
-----------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include "fortify.h"
|
|
#include "motreg.h"
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
pMotReg RegisterMotor(char *name, SicsInterp * pSics,
|
|
long (*NewSetValue) (void *pData, SConnection * pCon,
|
|
float farget),
|
|
int (*NewCheckStatus) (void *pData,
|
|
SConnection * pCon))
|
|
{
|
|
CommandList *pCom = NULL;
|
|
pIDrivable pDriv = NULL;
|
|
pMotReg pNew = NULL;
|
|
|
|
/*
|
|
find motor data structures
|
|
*/
|
|
pCom = FindCommand(pSics, name);
|
|
if (pCom == NULL) {
|
|
return NULL;
|
|
}
|
|
pDriv = GetDrivableInterface(pCom->pData);
|
|
if (pDriv == NULL) {
|
|
return NULL;
|
|
}
|
|
|
|
/*
|
|
everything seems OK. Allocate data structure and initialize
|
|
*/
|
|
pNew = (pMotReg) malloc(sizeof(MotReg));
|
|
if (pNew == NULL) {
|
|
return NULL;
|
|
}
|
|
memset(pNew, 0, sizeof(MotReg));
|
|
|
|
pNew->motorData = pCom->pData;
|
|
pNew->motorName = strdup(name);
|
|
pNew->originalSetValue = pDriv->SetValue;
|
|
pNew->originalCheckStatus = pDriv->CheckStatus;
|
|
pNew->iActive = 0;
|
|
|
|
pDriv->SetValue = NewSetValue;
|
|
pDriv->CheckStatus = NewCheckStatus;
|
|
|
|
return pNew;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------*/
|
|
void KillRegMot(void *pData)
|
|
{
|
|
pMotReg self = (pMotReg) pData;
|
|
|
|
if (self == NULL) {
|
|
return;
|
|
}
|
|
if (self->motorName != NULL) {
|
|
free(self->motorName);
|
|
}
|
|
free(self);
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
void SetRegMotTarget(pMotReg self, float fValue)
|
|
{
|
|
assert(self);
|
|
|
|
self->targetPosition = fValue;
|
|
self->iActive = 1;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
void CreateTargetString(pMotReg self, char pBueffel[80])
|
|
{
|
|
assert(self);
|
|
|
|
if (strlen(self->motorName) + 20 < 80) {
|
|
sprintf(pBueffel, " %s %12.4f ", self->motorName,
|
|
self->targetPosition);
|
|
}
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
int RegMotMatch(pMotReg self, char *name)
|
|
{
|
|
assert(self);
|
|
if (strcmp(self->motorName, name) == 0) {
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------*/
|
|
int StartRegMot(pMotReg self, SConnection * pCon, float fValue)
|
|
{
|
|
int ret;
|
|
long (*oldSet) (void *pmotorData, SConnection * pCon, float fValue);
|
|
pIDrivable pDriv = NULL;
|
|
char pBueffel[132];
|
|
|
|
|
|
assert(self);
|
|
pDriv = GetDrivableInterface(self->motorData);
|
|
assert(pDriv);
|
|
oldSet = pDriv->SetValue;
|
|
pDriv->SetValue = self->originalSetValue;
|
|
ret = StartDevice(pServ->pExecutor, self->motorName,
|
|
FindDescriptor(self->motorData),
|
|
self->motorData, pCon, SCGetRunLevel(pCon), fValue);
|
|
/*
|
|
snprintf(pBueffel,sizeof(pBueffel)-1,"anticollision started %s to %f",self->motorName,
|
|
fValue);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
*/
|
|
|
|
pDriv->SetValue = oldSet;
|
|
if (ret == 1) {
|
|
self->iActive = 1;
|
|
} else {
|
|
snprintf(pBueffel, 131, "ERROR: failed to start motor %s",
|
|
self->motorName);
|
|
SCWrite(pCon, pBueffel, eError);
|
|
self->iActive = 0;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------*/
|
|
int CheckRegMot(pMotReg self, SConnection * pCon)
|
|
{
|
|
int stat;
|
|
|
|
assert(self);
|
|
if (self->iActive) {
|
|
stat = self->originalCheckStatus(self->motorData, pCon);
|
|
if (stat != HWBusy) {
|
|
self->iActive = 0;
|
|
}
|
|
return stat;
|
|
} else {
|
|
return HWIdle;
|
|
}
|
|
}
|