linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
sics/motorsec.c

1040 lines
30 KiB
C
Raw Permalink Blame History

/**
* This is the implementation of a second generation motor. A second
* generation motor differs in various aspects from a first generation
* motor though it is functionally equivalent and even adheres to the
* same interface. The differing aspects are:
* - The second generation motor deos not use the ObPar array for parameter storage
* but the Hipadaba parameter system rooted in self->pDes->objectNode.
* - The second generation motor does not use the driver. The second generation motor
* implements some functionality but is largely a shell. Scripts are supposed to add
* more parameters and link to a corresponding ScriptContext object which takes care
* of actual talking to the hardware.
* - The old callback system is also not implemented: third parties which are interested
* to be notified of changes to the motor position shall register a callback.
*
* This also means that many of the fields in the motor data structure are not used in
* this module but are present to support first generation motors. This must be so as we cannot
* replace all existing driver with new ones quickly. Here a list of fields which have no use:
* - pDriver
* - ParArray
*
* We need three different position parameters to keep this under control:
* - the physical value which is the main motor node
* - the targetposition which is is the hardware target where the motor is supposed
* to be
* - The hardposition which is the actual hardware position. This is also the
* one which is responsible for talking to the motor.
*
* Then we need a status parameter which is some text which says what the motor
* is up to.
*
* A less obvious use of a callback is the HardUpdateCallback. It automatically
* updates the physical motor position, too.
*
* copyright: see file COPYRIGHT
*
* Mark Koennecke, December 2008
*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include "sics.h"
#include "sicshipadaba.h"
#include "sicsobj.h"
#include "motor.h"
#include "splitter.h"
#define ABS(x) (x < 0 ? -(x) : (x))
/*-------------------------------------------------------------------------*/
static void SecMotorSetError(pMotor self, char *text)
{
hdbValue v;
pHdb node;
node = GetHipadabaNode(self->pDescriptor->parNode, "error");
if(node != NULL){
v = MakeHdbText(strdup(text));
UpdateHipadabaPar(node,v,NULL);
Log(VERBOSE,"par","%s:error:%s", self->name,text);
ReleaseHdbValue(&v);
}
}
/*--------------------------------------------------------------------------*/
static int SecMotorGetPar(pMotor self, char *name, float *fVal)
{
char pBueffel[512];
hdbValue value;;
int status;
snprintf(pBueffel, 511, "%s ", name);
status = SICSHdbGetPar(self, NULL, pBueffel, &value);
*fVal = (float) value.v.doubleValue;
return status;
}
/*---------------------------------------------------------------------------*/
static int SecMotorSetPar(pMotor self, SConnection * pCon, char *name,
float fVal)
{
int status;
hdbValue value;
value = MakeHdbFloat(fVal);
status = SICSHdbSetPar(self, pCon, name, value);
if (status == 1) {
SCparChange(pCon);
Log(VERBOSE,"par","%s:%s:%f", self->name,name,fVal);
return 1;
} else {
return 0;
}
}
/*-------------------------------------------------------------------------*/
static void *MotorGetInterfaceSec(void *pData, int iID)
{
pMotor self = NULL;
self = (pMotor) pData;
assert(self);
if (iID == DRIVEID) {
return self->pDrivInt;
} else if (iID == CALLBACKINTERFACE) {
return self->pCall;
}
return NULL;
}
/*---------------------------------------------------------------------------*/
static long SecMotorRun(void *sulf, SConnection * pCon, float fNew)
{
pMotor self = (pMotor) sulf;
hdbValue v;
int accesscode, status;
pHdb node = NULL;
assert(SICSHdbGetPar(self, NULL, "accesscode", &v) == 1);
accesscode = (int)v.v.doubleValue;
if(!SCMatchRights(pCon, accesscode)){
return 0;
}
self->stopped = 0;
self->stopReported = 0;
self->fTarget = fNew;
SecMotorSetError(sulf,"None");
node = GetHipadabaNode(self->pDescriptor->parNode, "status");
if(node != NULL){
v = MakeHdbText(strdup("run"));
UpdateHipadabaPar(node,v,pCon);
ReleaseHdbValue(&v);
}
v = MakeHdbFloat(fNew);
status = SetHipadabaPar(self->pDescriptor->parNode, v, pCon);
return status;
}
/*--------------------------------------------------------------------------*/
static int SecMotorCheckBoundary(pMotor self, float fVal, float *fTarget,
char *pError, int iErrLen)
{
double fZero, fixed, lowerlim, upperlim, sign, offset;
float fHard, hupper, hlower;
char pBueffel[512];
hdbValue v;
assert(self);
assert(SICSHdbGetPar(self, NULL, "fixed", &v) == 1);
fixed = v.v.doubleValue;
assert(SICSHdbGetPar(self, NULL, "softzero", &v) == 1);
fZero = v.v.doubleValue;
assert(SICSHdbGetPar(self, NULL, "staticoffset", &v) == 1);
offset = v.v.doubleValue;
assert(SICSHdbGetPar(self, NULL, "softlowerlim", &v) == 1);
lowerlim = v.v.doubleValue;
assert(SICSHdbGetPar(self, NULL, "softupperlim", &v) == 1);
upperlim = v.v.doubleValue;
assert(SICSHdbGetPar(self, NULL, "sign", &v) == 1);
sign = v.v.doubleValue;
*fTarget = fVal;
/* check for fixed */
if (fixed >= 0) {
snprintf(pBueffel,sizeof(pBueffel)-1, "Motor %s is Fixed", self->name);
strlcpy(pError, pBueffel, iErrLen);
return 0; /* is this an error? */
}
/* check against software boundaries */
if (fVal > upperlim) {
snprintf(pBueffel,sizeof(pBueffel)-1, "%f violates upper software limit %f on %s",
fVal, upperlim, self->name);
strlcpy(pError, pBueffel, iErrLen);
return 0;
}
if (fVal < lowerlim) {
snprintf(pBueffel,sizeof(pBueffel)-1, "%f violates lower software limit %f on %s",
fVal, lowerlim, self->name);
strlcpy(pError, pBueffel, iErrLen);
return 0;
}
/* correct for zero point */
fZero += offset;
fZero = -fZero;
fHard = fVal - fZero;
/* apply sign */
fHard = fHard * sign;
/* check for hardware limits */
SecMotorGetPar(self, "hardlowerlim", &hlower);
SecMotorGetPar(self, "hardupperlim", &hupper);
if (fHard > hupper) {
snprintf(pBueffel,sizeof(pBueffel)-1, "%f violates upper hardware limit %f on %s",
fVal, hupper, self->name);
strlcpy(pError, pBueffel, iErrLen);
return 0;
}
if (fHard < hlower) {
snprintf(pBueffel,sizeof(pBueffel)-1, "%f violates lower hardware limit %f on %s",
fVal, hlower, self->name);
strlcpy(pError, pBueffel, iErrLen);
return 0;
}
*fTarget = fHard;
return 1;
}
/*--------------------------------------------------------------------------*/
static int SecMotorLimits(void *sulf, float fVal, char *error, int iErrLen)
{
float fHard;
pMotor self;
int status;
assert(sulf);
self = (pMotor) sulf;
status = SecMotorCheckBoundary(self, fVal, &fHard, error, iErrLen);
if(status != 1){
SecMotorSetError(sulf,error);
}
return status;
}
/*-----------------------------------------------------------------------*/
static int checkPosition(pMotor self, SConnection * pCon)
{
float precision, hard, target, maxretry;
pHdb node = NULL;
if (self->stopped) {
if(self->stopReported == 0){
SCPrintf(pCon, eLog, "WARNING: %s stopped", self->name);
self->stopReported = 1;
}
return HWFault;
}
if (SCGetInterrupt(pCon) != eContinue) {
return HWFault;
}
SecMotorGetPar(self, "hardposition", &hard);
SecMotorGetPar(self, "targetposition", &target);
SecMotorGetPar(self, "precision", &precision);
SecMotorGetPar(self, "maxretry", &maxretry);
if (ABS(target - hard) > precision) {
if (self->retryCount >= (int) maxretry) {
SCPrintf(pCon, eLogError,
"ERROR: aborting %s after %d retries, off position by %f",
self->name, (int) maxretry, target - hard);
node = GetHipadabaNode(self->pDescriptor->parNode, "status");
assert(node != NULL);
SecMotorSetError(self,"Aborted positioning after many retries");
UpdateHipadabaPar(node, MakeHdbText("error"), pCon);
return HWFault;
}
self->retryCount++;
SCPrintf(pCon, eLog, "WARNING: %s off position by %f, restarting",
self->name, target - hard);
node = GetHipadabaNode(self->pDescriptor->parNode, "status");
assert(node != NULL);
UpdateHipadabaPar(node, MakeHdbText("run"), pCon);
node = GetHipadabaNode(self->pDescriptor->parNode, "hardposition");
assert(node != NULL);
SetHipadabaPar(node, MakeHdbFloat(target), pCon);
return HWBusy;
}
return HWIdle;
}
/*-----------------------------------------------------------------------*/
static void handleMoveCallback(pMotor self, SConnection * pCon)
{
float movecount;
pHdb node = NULL;
hdbValue v;
SecMotorGetPar(self, "movecount", &movecount);
self->posCount++;
if (self->posCount > (int) movecount) {
node = GetHipadabaNode(self->pDescriptor->parNode, "hardposition");
GetHipadabaPar(node, &v, pCon);
UpdateHipadabaPar(node, v, pCon);
Log(VERBOSE,"par","%s:hardposition:%f",self->name,v.v.doubleValue);
self->posCount = 0;
}
}
/*-----------------------------------------------------------------------*/
static int SecMotorStatus(void *sulf, SConnection * pCon)
{
pMotor self = NULL;
int status;
pHdb node = NULL;
hdbValue v;
float interrupt = 0.;
char error[132];
assert(sulf);
self = (pMotor) sulf;
node = GetHipadabaNode(self->pDescriptor->parNode, "status");
assert(node != NULL);
status = GetHipadabaPar(node, &v, pCon);
if (status != 1) {
return HWFault;
}
if (v.v.text == NULL) {
return HWBusy;
}
if (strstr(v.v.text, "idle") != NULL) {
status = checkPosition(self, pCon);
} else if (strstr(v.v.text, "run") != NULL) {
handleMoveCallback(self, pCon);
status = HWBusy;
} else if (strstr(v.v.text, "poserror") != NULL) {
SCPrintf(pCon,eWarning,"WARNING: %s repositioned", self->name);
status = checkPosition(self, pCon);
} else if (strstr(v.v.text, "restart") != NULL) {
SCPrintf(pCon,eLog,"WARNING: restarting motor %s", self->name);
SecMotorRun(self,pCon,self->fTarget);
return HWBusy;
} else if (strstr(v.v.text, "error") != NULL) {
if(GetHdbProperty(node,"geterror",error,sizeof(error)) == 1){
SecMotorSetError(sulf,error);
}
node = GetHipadabaNode(self->pDescriptor->parNode, "targetposition");
if(GetHdbProperty(node,"geterror",error,sizeof(error)) == 1){
SecMotorSetError(sulf,error);
}
status = HWFault;
} else {
SCPrintf(pCon, eError, "ERROR: unknown motor status %s found",
v.v.text);
status = HWFault;
}
ReleaseHdbValue(&v);
/*
* when terminating: force an update of the position.
*/
switch (status) {
case HWFault:
self->posCount = 10000;
handleMoveCallback(self, pCon);
SecMotorGetPar(self, "interruptmode", &interrupt);
if (SCGetInterrupt(pCon) < (int) interrupt) {
SCSetInterrupt(pCon, (int) interrupt);
}
break;
case HWIdle:
self->posCount = 10000;
handleMoveCallback(self, pCon);
break;
}
return status;
}
/*---------------------------------------------------------------------------*/
static float SecMotorGetValue(void *pData, SConnection * pCon)
{
int status;
pMotor self = (pMotor) pData;
hdbValue v;
char error[132];
assert(pData);
status = GetHdbProperty(self->pDescriptor->parNode,"geterror", error,sizeof(error));
if(status == 1 && strcmp(error,"none") != 0) {
SCPrintf(pCon,eError,"ERROR: Failed to read %s with %s", self->name, error);
return -9999999.99;
}
status = GetHipadabaPar(self->pDescriptor->parNode, &v, pCon);
if (status != 1) {
return -9999999.99;
} else {
return (float) v.v.doubleValue;
}
}
/*------------------------------------------------------------------------*/
static int SecMotorHalt(void *sulf)
{
pMotor self;
pHdb node = NULL, par[0];
SICSOBJFunc haltFunc = NULL;
int status;
assert(sulf);
self = (pMotor) sulf;
node = GetHipadabaNode(self->pDescriptor->parNode, "halt");
assert(node != NULL);
haltFunc = (SICSOBJFunc) node->value.v.func;
assert(haltFunc != NULL);
self->stopped = 1;
self->pDrivInt->iErrorCount--;
if(self->pDrivInt->iErrorCount < 0){
self->pDrivInt->iErrorCount = 0;
}
return haltFunc((pSICSOBJ) self, pServ->dummyCon, node, par, 0);
}
/*--------------------------------------------------------------------------*/
static int SecMotorGetHardPosition(struct __Motor *self,
SConnection * pCon, float *fVal)
{
hdbValue v;
int status;
pHdb node = NULL;
node = GetHipadabaNode(self->pDescriptor->parNode, "hardposition");
assert(node != NULL);
status = GetHipadabaPar(node, &v, pCon);
*fVal = (float) v.v.doubleValue;
return status;
}
/*---------------------------------------------------------------------------*/
static void AddMotorPar(pHdb node, int priv, char *name)
{
pHdb child = NULL;
child = MakeSICSHdbPar(name, priv, MakeHdbFloat(.0));
if (child != NULL) {
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
}
}
/*---------------------------------------------------------------------------*/
static float hardToSoftPosition(pMotor self, float hard)
{
float sign, zero, fVal, offset;
SecMotorGetPar(self, "sign", &sign);
SecMotorGetPar(self, "softzero", &zero);
SecMotorGetPar(self, "staticoffset", &offset);
fVal = hard;
if (sign < 0) {
fVal += zero + offset;
} else {
fVal -= zero + offset;
}
fVal *= sign;
return fVal;
}
/*---------------------------------------------------------------------------*/
static hdbCallbackReturn SecMotorCallback(pHdb node, void *userData,
pHdbMessage message)
{
SConnection *pCon = NULL;
pHdbDataMessage mm = NULL;
hdbValue v;
pHdb child = NULL;
pMotor self = NULL;
float fHard, fVal, sign, zero;
char pBueffel[512], pError[132], *pPtr = NULL;
int status;
self = (pMotor) userData;
assert(self != NULL);
mm = GetHdbSetMessage(message);
if (mm != NULL) {
pCon = (SConnection *) mm->callData;
v = *(mm->v);
/*
* check permission
*/
SecMotorGetPar(self, "accesscode", &fVal);
if (!SCMatchRights(pCon, (int) fVal)) {
snprintf(pBueffel,sizeof(pBueffel)-1, "ERROR: You are not authorised to move motor %s",
self->name);
SCWrite(pCon, pBueffel, eError);
SCSetInterrupt(pCon, eAbortBatch);
return hdbAbort;
}
/*
* check limits
*/
status = SecMotorCheckBoundary(self, (float) v.v.doubleValue,
&fHard, pError, 131);
if (status != 1) {
snprintf(pBueffel, 511, "ERROR: %s", pError);
SCWrite(pCon, pBueffel, eWarning);
SCSetInterrupt(pCon, eAbortOperation);
return hdbAbort;
}
/*
* check the motor bad flag
*/
SecMotorGetPar(self, "ignorefault", &fVal);
if ((int) fVal > 0) {
snprintf(pBueffel, 511, "WARNING: motor %s is unreliable",
self->name);
SCWrite(pCon, pBueffel, eWarning);
self->errorCount = 0;
self->pDrivInt->iErrorCount = 0;
}
/*
* check for alarm condition
*/
SecMotorGetPar(self, "failafter", &fVal);
if (self->pDrivInt->iErrorCount >= (int) fVal) {
/* big alarm */
ServerWriteGlobal("ERROR: !!! MOTOR ALARM !!! MOTOR ALARM !!!",
eError);
snprintf(pBueffel,sizeof(pBueffel)-1,
"ERROR: too many position errors counted at motor %s",
self->name);
ServerWriteGlobal(pBueffel, eError);
SCSetInterrupt(pCon, eAbortBatch);
self->pDrivInt->iErrorCount = 0;
child = GetHipadabaNode(self->pDescriptor->parNode, "status");
UpdateHipadabaPar(child, MakeHdbText("error"), pCon);
return hdbAbort;
}
self->posFaultCount = 0;
self->retryCount = 0;
self->stopped = 0;
self->posCount = 0;
child = GetHipadabaNode(self->pDescriptor->parNode, "status");
UpdateHipadabaPar(child, MakeHdbText("run"), pCon);
child = GetHipadabaNode(self->pDescriptor->parNode, "targetposition");
UpdateHipadabaPar(child, MakeHdbFloat(fHard), pCon);
Log(VERBOSE,"par","%s:targetposition:%f", self->name,fHard);
child = GetHipadabaNode(self->pDescriptor->parNode, "hardposition");
SetHipadabaPar(child, MakeHdbFloat(fHard), pCon);
return hdbContinue;
}
mm = GetHdbGetMessage(message);
if (mm != NULL) {
pCon = (SConnection *) mm->callData;
SecMotorGetPar(self, "hardposition", &fVal);
child = GetHipadabaNode(self->pDescriptor->parNode, "hardposition");
if((pPtr = GetHdbProp(child,"geterror")) != NULL){
SetHdbProperty(node,"geterror",pPtr);
} else {
SetHdbProperty(node,"geterror",NULL);
}
fVal = hardToSoftPosition(self, fVal);
node->value.v.doubleValue = fVal;
mm->v->v.doubleValue = fVal;
return hdbContinue;
}
return hdbContinue;
}
/*--------------------------------------------------------------------------*/
static hdbCallbackReturn HardUpdateCallback(pHdb node, void *userData,
pHdbMessage message)
{
pHdbDataMessage mm = NULL;
pHdbPropertyChange pm = NULL;
pMotor self = (pMotor) userData;
float fVal;
hdbValue v;
assert(self != NULL);
mm = GetHdbUpdateMessage(message);
if (mm != NULL) {
v = *mm->v;
if(ABS(v.v.doubleValue - node->value.v.doubleValue) > .01){
Log(VERBOSE,"par","%s:hardposition:%f", self->name,v.v.doubleValue);
}
fVal = hardToSoftPosition(self, (float) v.v.doubleValue);
v.v.doubleValue = fVal;
UpdateHipadabaPar(self->pDescriptor->parNode, v, mm->callData);
return hdbContinue;
}
/*
forward geterror
*/
pm = GetPropertyChangeMessage(message);
if(pm != NULL){
if(strstr(pm->key,"geterror") != NULL){
SetHdbProperty(self->pDescriptor->parNode,pm->key, pm->value);
}
}
return hdbContinue;
}
/*--------------------------------------------------------------------------*/
static hdbCallbackReturn StatusUpdateCallback(pHdb node, void *userData,
pHdbMessage message)
{
pHdbDataMessage mm = NULL;
pHdbPropertyChange pm = NULL;
pMotor self = (pMotor) userData;
float fVal;
hdbValue v;
assert(self != NULL);
mm = GetHdbUpdateMessage(message);
if (mm != NULL) {
v = *mm->v;
if(strcmp(v.v.text,node->value.v.text) != 0){
UpdateHipadabaPar(self->pDescriptor->parNode, v, mm->callData);
Log(VERBOSE,"par","%s:status:%s", self->name,v.v.text);
}
return hdbContinue;
}
/*
forward geterror
*/
pm = GetPropertyChangeMessage(message);
if(pm != NULL){
if(strstr(pm->key,"geterror") != NULL){
SetHdbProperty(self->pDescriptor->parNode,pm->key, pm->value);
}
}
return hdbContinue;
}
/*--------------------------------------------------------------------------*/
static hdbCallbackReturn SecMotorSignCallback(pHdb node, void *userData,
pHdbMessage message)
{
pMotor self = NULL;
SConnection *pCon = NULL;
pHdb zero = NULL;
double value;
pHdbDataMessage mm = NULL;
float limit;
hdbValue v;
self = (pMotor) userData;
mm = GetHdbSetMessage(message);
if (mm != NULL) {
pCon = (SConnection *) mm->callData;
if (!SCMatchRights(pCon, usMugger)) {
return hdbAbort;
}
v = *mm->v;
if (ABS(v.v.doubleValue) - 1. > .01) {
if (pCon != NULL) {
SCWrite(pCon, "ERROR: invalid sign value", eError);
return hdbAbort;
}
}
SecMotorGetPar(self, "softlowerlim", &limit);
limit *= v.v.doubleValue;
SecMotorSetPar(self, pCon, "softupperlim", limit);
SecMotorGetPar(self, "softupperlim", &limit);
limit *= v.v.doubleValue;
SecMotorSetPar(self, pCon, "softlowerlim", limit);
SecMotorSetPar(self, pCon, "softzero", .0);
UpdateHipadabaPar(node, v, pCon);
return hdbContinue;
}
return hdbContinue;
}
/*---------------------------------------------------------------------------*/
static hdbCallbackReturn SecMotorZeroCallback(pHdb node, void *userData,
pHdbMessage message)
{
pMotor self = NULL;
float limit, oldZero, diff;
SConnection *pCon = NULL;
pHdbDataMessage mm = NULL;
hdbValue v;
self = (pMotor) userData;
assert(self != NULL);
mm = GetHdbSetMessage(message);
if (mm != NULL) {
pCon = (SConnection *) mm->callData;
if (!SCMatchRights(pCon, usUser)) {
return hdbAbort;
}
v = *mm->v;
SecMotorGetPar(self, "softzero", &oldZero);
diff = v.v.doubleValue - oldZero;
SecMotorGetPar(self, "softupperlim", &limit);
limit -= diff;
SecMotorSetPar(self, pCon, "softupperlim", limit);
SecMotorGetPar(self, "softlowerlim", &limit);
limit -= diff;
SecMotorSetPar(self, pCon, "softlowerlim", limit);
UpdateHipadabaPar(node, v, pCon);
return hdbContinue;
}
return hdbContinue;
}
/*--------------------------------------------------------------------------*/
typedef struct {
char *pName;
SConnection *pCon;
float lastValue;
} MotInfo, *pMotInfo;
/*--------------------------------------------------------------------------*/
static void KillInfo(void *pData)
{
pMotInfo self = NULL;
assert(pData);
self = (pMotInfo) pData;
if (self->pName) {
free(self->pName);
}
if (self->pCon != NULL) {
SCDeleteConnection(self->pCon);
}
free(self);
}
/*-------------------------------------------------------------------------*/
static hdbCallbackReturn InterestCallback(pHdb node, void *userData,
pHdbMessage message)
{
pHdbDataMessage mm = NULL;
pMotor self = (pMotor) userData;
float fVal;
hdbValue v;
pMotInfo priv = (pMotInfo)userData;
assert(self != NULL);
mm = GetHdbUpdateMessage(message);
if (mm != NULL) {
v = *mm->v;
if(!SCisConnected(priv->pCon)){
return hdbKill;
}
if(ABS(v.v.doubleValue - priv->lastValue) > .1) {
SCPrintf(priv->pCon,eValue,"%s.position = %f",
priv->pName, v.v.doubleValue);
priv->lastValue = v.v.doubleValue;
}
return hdbContinue;
}
return hdbContinue;
}
/*---------------------------------------------------------------------------*/
static int InterestCmd(pSICSOBJ ccmd, SConnection * con,
Hdb * cmdNode, Hdb * par[], int nPar)
{
pMotInfo priv = NULL;
priv = malloc(sizeof(MotInfo));
if(priv == NULL){
SCWrite(con,"ERROR: out of memory registering interest",eError);
return 0;
}
if(nPar >= 1 && (strcmp(par[0]->value.v.text,"UNKNOWN") != 0)) {
priv->pName = strdup(par[0]->value.v.text);
} else {
priv->pName = strdup(ccmd->objectNode->name);
}
priv->lastValue = .0;
priv->pCon = SCCopyConnection(con);
AppendHipadabaCallback(ccmd->objectNode,
MakeHipadabaCallback(InterestCallback,priv,KillInfo));
SCSendOK(con);
return 1;
}
/*---------------------------------------------------------------------------*/
pMotor SecMotorInit(char *name)
{
pMotor pM = NULL;
pHdb node = NULL, child = NULL;
hdbValue v;
assert(name);
/* get memory */
pM = (pMotor) malloc(sizeof(Motor));
if (!pM) {
return NULL;
}
memset(pM, 0, sizeof(Motor));
/* initialise object descriptor */
pM->pDescriptor = CreateDescriptor("Motor");
if (!pM->pDescriptor) {
free(pM);
return NULL;
}
pM->pDescriptor->GetInterface = MotorGetInterfaceSec;
pM->pDescriptor->SaveStatus = SaveSICSOBJ;
pM->pDescriptor->parNode = MakeSICSHdbPar(name, usSpy, MakeHdbFloat(.0));
if (pM->pDescriptor->parNode == NULL) {
free(pM);
return NULL;
}
node = pM->pDescriptor->parNode;
SetHdbProperty(node,"type","drivable");
SetHdbProperty(node,"sicsdev", name);
pM->objectNode = node;
AppendHipadabaCallback(pM->pDescriptor->parNode,
MakeHipadabaCallback(SecMotorCallback, pM, NULL));
/* copy arguments */
pM->name = strdup(name);
/*
* install parameters
*/
child = MakeSICSHdbPar("targetposition", usInternal, MakeHdbFloat(.0));
if (child == NULL) {
return (NULL);
}
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeHipadabaNode("hardposition", HIPFLOAT, 1);
if (child == NULL) {
return (NULL);
}
AddHipadabaChild(node, child, NULL);
SetHdbProperty(child, "motname", name);
AppendHipadabaCallback(child,
MakeHipadabaCallback(HardUpdateCallback, pM,
NULL));
child = MakeHipadabaNode("sign", HIPFLOAT, 1);
SetHdbProperty(child, "__save", "true");
SetHdbProperty(child, "priv", "user");
AddHipadabaChild(node, child, NULL);
AppendHipadabaCallback(child,
MakeHipadabaCallback(SecMotorSignCallback, pM,
NULL));
UpdateHipadabaPar(child, MakeHdbFloat(1.), NULL);
child = MakeHipadabaNode("softzero", HIPFLOAT, 1);
SetHdbProperty(child, "__save", "true");
SetHdbProperty(child, "priv", "user");
AddHipadabaChild(node, child, NULL);
AppendHipadabaCallback(child,
MakeHipadabaCallback(SecMotorZeroCallback, pM,
NULL));
child = MakeHipadabaNode("hardlowerlim", HIPFLOAT, 1);
AddHipadabaChild(node, child, NULL);
child = MakeHipadabaNode("hardupperlim", HIPFLOAT, 1);
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("softlowerlim", usUser, MakeHdbFloat(.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("softupperlim", usUser, MakeHdbFloat(.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("fixed", usUser, MakeHdbFloat(-1.));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("interruptmode", usMugger, MakeHdbFloat(.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("precision", usMugger, MakeHdbFloat(.01));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("accesscode", usMugger,
MakeHdbFloat((double) usUser));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("failafter", usMugger,
MakeHdbFloat((double) 3.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("maxretry", usMugger, MakeHdbFloat((double) 3.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("ignorefault", usMugger,
MakeHdbFloat((double) .0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("movecount", usMugger,
MakeHdbFloat((double) 10.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeSICSHdbPar("staticoffset", usMugger,
MakeHdbFloat((double) 0.0));
SetHdbProperty(child, "__save", "true");
AddHipadabaChild(node, child, NULL);
child = MakeHipadabaNode("status", HIPTEXT, 1);
SetHdbProperty(child, "motname", name);
AppendHipadabaCallback(child,
MakeHipadabaCallback(StatusUpdateCallback, pM,
NULL));
AddHipadabaChild(node, child, NULL);
child = MakeHipadabaNode("error", HIPTEXT, 1);
AddHipadabaChild(node, child, NULL);
child = AddSICSHdbPar(node,"interest", usUser, MakeSICSFunc(InterestCmd));
AddSICSHdbPar(child, "name", usUser, MakeHdbText("UNKNOWN"));
pM->endScriptID = 0;
/* initialise Drivable interface */
pM->pDrivInt = CreateDrivableInterface();
if (!pM->pDrivInt) {
DeleteDescriptor(pM->pDescriptor);
free(pM);
return NULL;
}
pM->pDrivInt->SetValue = SecMotorRun;
pM->pDrivInt->CheckLimits = SecMotorLimits;
pM->pDrivInt->CheckStatus = SecMotorStatus;
pM->pDrivInt->GetValue = SecMotorGetValue;
pM->pDrivInt->Halt = SecMotorHalt;
/*
* initialize motor function pointers
*/
pM->MotorGetPar = SecMotorGetPar;
pM->MotorSetPar = SecMotorSetPar;
pM->MotorGetHardPosition = SecMotorGetHardPosition;
/* initialise callback interface */
pM->pCall = CreateCallBackInterface();
if (!pM->pCall) {
MotorKill(pM);
return NULL;
}
/* done */
return pM;
}
/*--------------------------------------------------------------------------*/
static void SecMotorKill(void *data)
{
pMotor self = (pMotor) data;
if (self == NULL) {
return;
}
if (self->name)
free(self->name);
if (self->pDrivInt) {
free(self->pDrivInt);
}
RemoveHdbNodeFromParent(self->objectNode,NULL);
if (self->pCall) {
DeleteCallBackInterface(self->pCall);
}
/* kill Descriptor */
DeleteDescriptor(self->pDescriptor);
free(self);
}
/*--------------------------------------------------------------------------*/
int SecMotorFactory(SConnection * pCon, SicsInterp * pSics, void *pData,
int argc, char *argv[])
{
pMotor pNew = NULL;
if (argc < 2) {
SCWrite(pCon, "ERROR: need name for new motor", eError);
return 0;
}
pNew = SecMotorInit(argv[1]);
if (pNew == NULL) {
SCWrite(pCon, "ERROR: out of memory creating motor", eError);
return 0;
}
return AddCommand(pSics, argv[1], InterInvokeSICSOBJ,
SecMotorKill, pNew);
}
/*--------------------------------------------------------------------------
everywhere we need to find a motor for a name quite frequently.......
----------------------------------------------------------------------------*/
pMotor FindMotor(SicsInterp * pSics, char *name)
{
CommandList *pC;
pMotor pMot;
pC = FindCommand(pSics, name);
if (!pC) {
return NULL;
}
pMot = (pMotor) pC->pData;
if (!pMot) {
return NULL;
}
if (strcmp(pMot->pDescriptor->name, "Motor") != 0 ) {
return NULL;
}
return pMot;
}
Reference in New Issue View Git Blame Copy Permalink