linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
1770c0e2a88d8ecef2372420856c4d58efe360b5
sics/motorsec.c
koennecke 361ee9ebea - Reworked the connection object and the IO system 
- Reworked the support for TRICS
- Added a second generation motor
2009-02-03 08:05:39 +00:00

750 lines
22 KiB
C
Raw 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 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);
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;
v = MakeHdbFloat(fNew);
return SetHipadabaPar(self->pDescriptor->parNode,v,pCon);
}
/*--------------------------------------------------------------------------*/
static int SecMotorCheckBoundary(pMotor self, float fVal, float *fTarget,
char *pError, int iErrLen){
double fZero, fixed, lowerlim, upperlim, sign;
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,
"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){
sprintf(pBueffel,"Motor %s is Fixed",self->name);
strncpy(pError,pBueffel,iErrLen);
return 0; /* is this an error? */
}
/* check against software boundaries */
if(fVal > upperlim){
sprintf(pBueffel,"%f violates upper software limit %f on %s",
fVal, upperlim,self->name);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
if(fVal < lowerlim){
sprintf(pBueffel,"%f violates lower software limit %f on %s",
fVal,lowerlim,self->name );
strncpy(pError,pBueffel,iErrLen);
return 0;
}
/* correct for zero point */
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){
sprintf(pBueffel,"%f violates upper hardware limit %f on %s",
fVal,hupper,self->name);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
if(fHard < hlower){
sprintf(pBueffel,"%f violates lower hardware limit %f on %s",
fVal,hlower,self->name);
strncpy(pError,pBueffel,iErrLen);
return 0;
}
*fTarget = fHard;
return 1;
}
/*--------------------------------------------------------------------------*/
static int SecMotorLimits(void *sulf, float fVal, char *error, int iErrLen){
float fHard;
pMotor self;
assert(sulf);
self = (pMotor)sulf;
return SecMotorCheckBoundary(self,fVal,&fHard,error,iErrLen);
}
/*-----------------------------------------------------------------------*/
static int checkPosition(pMotor self, SConnection *pCon){
float precision, hard, target, maxretry;
pHdb node = NULL;
if(SCGetInterrupt(pCon) != eContinue){
return HWFault;
}
if(self->stopped){
SCPrintf(pCon,eWarning,"WARNING: %s stopped", self->name);
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,eError,
"ERROR: Aborting %s after %d retries, off position by %f",
self->name, (int)maxretry, target - hard);
return HWFault;
}
self->retryCount++;
SCPrintf(pCon,eWarning,"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);
self->posCount = 0;
}
}
/*-----------------------------------------------------------------------*/
static int SecMotorStatus(void *sulf, SConnection *pCon){
pMotor self = NULL;
int status;
pHdb node = NULL;
hdbValue v;
float interrupt;
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){
status = checkPosition(self,pCon);
} else if(strstr(v.v.text,"error") != NULL){
status = HWFault;
} else {
SCPrintf(pCon,eError,"ERROR: unknown motor status %s found", v.v.text);
status = HWFault;
}
/*
* when terminating: force an update of the position.
*/
switch(status){
case HWFault:
self->posCount = 10000;
handleMoveCallback(self,pCon);
SecMotorGetPar(self,"interrupt",&interrupt);
if(SCGetInterrupt(pCon) < (int)interrupt){
SCSetInterrupt(pCon,(int)interrupt);
}
self->errorCount++;
break;
case HWIdle:
self->posCount = 10000;
handleMoveCallback(self,pCon);
self->errorCount = 0;
break;
}
return status;
}
/*---------------------------------------------------------------------------*/
static float SecMotorGetValue(void *pData, SConnection *pCon){
int status;
pMotor self = (pMotor)pData;
hdbValue v;
assert(pData);
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;
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;
SecMotorGetPar(self,"sign", &sign);
SecMotorGetPar(self,"softzero", &zero);
fVal = hard;
if(sign < 0){
fVal += zero;
} else {
fVal -= zero;
}
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];
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)){
sprintf(pBueffel,"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;
}
/*
* check for alarm condition
*/
SecMotorGetPar(self,"failafter",&fVal);
if(self->errorCount > (int)fVal){
/* big alarm */
ServerWriteGlobal("ERROR: !!! MOTOR ALARM !!! MOTOR ALARM !!!",eError);
sprintf(pBueffel,
"ERROR: too many position errors counted at motor %s",
self->name);
ServerWriteGlobal(pBueffel,eError);
SCSetInterrupt(pCon,eAbortBatch);
self->errorCount = 0;
return hdbAbort;
}
self->posFaultCount = 0;
self->retryCount = 0;
self->stopped = 0;
self->posCount = 0;
child = GetHipadabaNode(self->pDescriptor->parNode,"targetposition");
UpdateHipadabaPar(child,MakeHdbFloat(fHard),pCon);
child = GetHipadabaNode(self->pDescriptor->parNode,"status");
UpdateHipadabaPar(child,MakeHdbText("run"), pCon);
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);
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;
pMotor self = (pMotor)userData;
float fVal;
hdbValue v;
assert(self != NULL);
mm = GetHdbUpdateMessage(message);
if(mm != NULL){
v = *mm->v;
fVal = hardToSoftPosition(self,(float)v.v.doubleValue);
v.v.doubleValue = fVal;
UpdateHipadabaPar(self->pDescriptor->parNode, v, mm->callData);
return hdbContinue;
}
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,"softlowerlim",limit);
SecMotorGetPar(self,"softupperlim",&limit);
limit *= v.v.doubleValue;
SecMotorSetPar(self,pCon,"softupperlim",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;
}
/*---------------------------------------------------------------------------*/
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;
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");
AddHipadabaChild(node,child,NULL);
AppendHipadabaCallback(child,
MakeHipadabaCallback(SecMotorSignCallback,pM,NULL));
UpdateHipadabaPar(child,MakeHdbFloat(1.),NULL);
child = MakeHipadabaNode("softzero",HIPFLOAT, 1);
SetHdbProperty(child,"__save", "true");
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(.1));
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 = MakeHipadabaNode("status",HIPTEXT,1);
SetHdbProperty(child,"motname", name);
AddHipadabaChild(node,child,NULL);
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);
}
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);
}
Reference in New Issue View Git Blame Copy Permalink