a5c2da6acf
- Changes to Hipadaba - Added project to histogram memory code - Started regression testing code - Added hill climbing as optimization method to optimise
1352 lines
41 KiB
C
1352 lines
41 KiB
C
/*------------------------------------------------------------------------
|
|
M O T O R S
|
|
|
|
This file implements the SICS motor handling. This is the logical
|
|
level, the nitty gritty hardware interface is in the driver.
|
|
|
|
This is a totally revamped version which uses the Hipadaba for
|
|
data storage.
|
|
|
|
copyright: see file COPYRIGHT
|
|
|
|
Mark Koennecke, July 2006
|
|
-----------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <tcl.h>
|
|
#include "sics.h"
|
|
#include "devexec.h"
|
|
#include "motor.h"
|
|
#include "splitter.h"
|
|
#include "status.h"
|
|
#include "servlog.h"
|
|
#include "tclmotdriv.h"
|
|
#include "site.h"
|
|
#include "sicshipadaba.h"
|
|
#include "stptok.h"
|
|
/*-------------------------------------------------------------------------
|
|
some lokal defines
|
|
*/
|
|
#define ZEROINACTIVE 0
|
|
#define INTCONT 0.
|
|
#define INTSCAN 1.
|
|
#define INTBATCH 2.
|
|
#define INTHALT 3.
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
/*------------------------------------------------------------------------
|
|
a tiny structure used in CallBack work
|
|
*/
|
|
typedef struct {
|
|
float fVal;
|
|
char *pName;
|
|
} MotCallback;
|
|
/*-------------------------------------------------------------------------*/
|
|
static void *MotorGetInterface(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 int MotorHalt(void *sulf){
|
|
pMotor self;
|
|
|
|
assert(sulf);
|
|
self = (pMotor)sulf;
|
|
|
|
/* reduce the error count by 1. This is because the driver is
|
|
expected to return an error when the motor had been stopped.
|
|
However, a stop is usually a consequence of a user intervention
|
|
or program logic. This prevents to a false motor alarm when the
|
|
motor was repeatedly stopped for other reasons.
|
|
*/
|
|
self->pDrivInt->iErrorCount--;
|
|
if(self->pDrivInt->iErrorCount < 0)
|
|
self->pDrivInt->iErrorCount = 0;
|
|
|
|
self->stopped = 1;
|
|
return self->pDriver->Halt((void *)self->pDriver);
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int MotorLimits(void *sulf, float fVal, char *error, int iErrLen){
|
|
float fHard;
|
|
pMotor self;
|
|
|
|
assert(sulf);
|
|
|
|
self = (pMotor)sulf;
|
|
|
|
return MotorCheckBoundary(self,fVal,&fHard,error,iErrLen);
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
/*---------------------------------------------------------------------------*/
|
|
static float MotorGetValue(void *pData, SConnection *pCon){
|
|
int iRet;
|
|
float fVal = 0.;
|
|
|
|
assert(pData);
|
|
iRet = MotorGetHardPosition((pMotor)pData,pCon,&fVal);
|
|
if(iRet != OKOK){
|
|
fVal = -9999999.99;
|
|
}
|
|
return fVal;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int MotorSaveStatus(void *pData, char *name, FILE *fd){
|
|
pMotor self = NULL;
|
|
char pBueffel[512];
|
|
|
|
assert(pData);
|
|
assert(fd);
|
|
|
|
self= (pMotor)pData;
|
|
fprintf(fd,"# Motor %s\n",name);
|
|
strncpy(pBueffel,name,511);
|
|
strncat(pBueffel," ",511);
|
|
SaveSICSHipadaba(fd,self->pDescriptor->parNode,pBueffel);
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void MotorInterrupt(SConnection *pCon, int iVal){
|
|
if(SCGetInterrupt(pCon) < iVal){
|
|
SCSetInterrupt(pCon,iVal);
|
|
}
|
|
}
|
|
/*---------------------------------------------------------------------*/
|
|
static int statusRunTo(pMotor self, SConnection *pCon)
|
|
{
|
|
char pBueffel[256];
|
|
float posCount;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,"maxretry",
|
|
&posCount) == 1);
|
|
|
|
if(self->retryCount >= (int)posCount)
|
|
{
|
|
snprintf(pBueffel,255,"ERROR: aborting motor %s after %d retries",
|
|
self->name, self->retryCount);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return HWFault;
|
|
}
|
|
if(SCGetInterrupt(pCon) != eContinue){
|
|
return HWFault;
|
|
}
|
|
self->retryCount++;
|
|
snprintf(pBueffel,255,"WARNING: restarting %s, %d time",
|
|
self->name,self->retryCount);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
self->pDriver->RunTo(self->pDriver,self->fTarget);
|
|
return HWBusy;
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
static int checkPosition(pMotor self, SConnection *pCon)
|
|
{
|
|
float fHard, precision;
|
|
char pBueffel[132];
|
|
int status;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"precision",&precision) == 1);
|
|
|
|
MotorGetHardPosition(self,pCon,&fHard);
|
|
self->fPosition = fHard;
|
|
if(ABS(fHard - self->fTarget) > precision){
|
|
if(SCGetInterrupt(pCon) != eContinue){
|
|
return HWFault;
|
|
}
|
|
if(self->stopped){
|
|
snprintf(pBueffel,131,"WARNING: %s stopped", self->name);
|
|
SCWrite(pCon,pBueffel, eWarning);
|
|
return HWFault;
|
|
}
|
|
snprintf(pBueffel,131,"WARNING: %s off position by %f",
|
|
self->name, ABS(fHard - self->fTarget));
|
|
SCWrite(pCon,pBueffel, eWarning);
|
|
status = statusRunTo(self,pCon);
|
|
return status;
|
|
}
|
|
return HWIdle;
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
static void finishDriving(pMotor self, SConnection *pCon){
|
|
MotCallback sCall;
|
|
MotorGetSoftPosition(self,pCon,&sCall.fVal);
|
|
sCall.pName = self->name;
|
|
InvokeCallBack(self->pCall, MOTDRIVE, &sCall); /* send also very last position */
|
|
InvokeCallBack(self->pCall, MOTEND, &sCall);
|
|
UpdateHipadabaPar(self->pDescriptor->parNode,
|
|
MakeHdbFloat((double)sCall.fVal),pCon);
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
static int reportAndFixError(pMotor self, SConnection *pCon){
|
|
char pBueffel[256], pError[131];
|
|
int iCode, iRet, newStatus;
|
|
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, self->fTarget);
|
|
switch(iRet){
|
|
case MOTFAIL:
|
|
snprintf(pBueffel,255,"ERROR: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
newStatus = HWFault;
|
|
break;
|
|
case MOTREDO:
|
|
snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
newStatus = statusRunTo(self,pCon);
|
|
break;
|
|
case MOTOK:
|
|
snprintf(pBueffel,255,"WARNING: %s on %s",pError,self->name);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
newStatus = HWIdle;
|
|
break;
|
|
default:
|
|
SCWrite(pCon,"WARNING: bad status code in motor.c:reportAndFixError",
|
|
eWarning);
|
|
SCWrite(pCon,"You may continue, but show this to a SICS programmer",
|
|
eWarning);
|
|
newStatus = HWIdle;
|
|
break;
|
|
}
|
|
return newStatus;
|
|
}
|
|
/*---------------------------------------------------------------------*/
|
|
static int evaluateStatus(pMotor self, SConnection *pCon){
|
|
int iRet, iCode, newStatus;
|
|
MotCallback sCall;
|
|
char pBueffel[256], pError[132];
|
|
float fHard, ignorefault, interrupt;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"ignorefault",&ignorefault) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"interruptmode",&interrupt) == 1);
|
|
|
|
iRet = self->pDriver->GetStatus(self->pDriver);
|
|
newStatus = iRet;
|
|
switch(iRet){
|
|
case OKOK:
|
|
case HWIdle:
|
|
newStatus = checkPosition(self,pCon);
|
|
if(newStatus != HWBusy){
|
|
finishDriving(self,pCon);
|
|
}
|
|
break;
|
|
case HWFault:
|
|
newStatus = reportAndFixError(self,pCon);
|
|
break;
|
|
case HWPosFault:
|
|
newStatus = reportAndFixError(self,pCon);
|
|
if(newStatus == HWFault && ignorefault < 1){
|
|
newStatus = HWPosFault;
|
|
}
|
|
if(newStatus == HWIdle || newStatus == OKOK){
|
|
newStatus = checkPosition(self,pCon);
|
|
if(newStatus != HWBusy){
|
|
finishDriving(self,pCon);
|
|
}
|
|
}
|
|
break;
|
|
case HWBusy:
|
|
newStatus = HWBusy;
|
|
break;
|
|
case HWWarn:
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
snprintf(pBueffel,255,"WARNING: motor reported: %s", pError);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
newStatus = HWIdle;
|
|
break;
|
|
default:
|
|
SCWrite(pCon,"WARNING: Bad status in motor.c:evaluatStatus",eWarning);
|
|
SCWrite(pCon,"You may continue, but show this to a SICS programmer",
|
|
eWarning);
|
|
break;
|
|
}
|
|
if(newStatus == HWFault){
|
|
MotorInterrupt(pCon,(int)interrupt);
|
|
self->retryCount = 0;
|
|
}
|
|
return newStatus;
|
|
}
|
|
/*---------------------------------------------------------------------*/
|
|
static void handleMoveCallback(pMotor self, SConnection *pCon){
|
|
MotCallback sCall;
|
|
float movecount;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"movecount",&movecount) == 1);
|
|
|
|
self->posCount++;
|
|
if(self->posCount >= (int)movecount){
|
|
MotorGetSoftPosition(self,pCon,&sCall.fVal);
|
|
sCall.pName = self->name;
|
|
InvokeCallBack(self->pCall, MOTDRIVE, &sCall);
|
|
self->posCount = 0;
|
|
UpdateHipadabaPar(self->pDescriptor->parNode,
|
|
MakeHdbFloat((double)sCall.fVal),pCon);
|
|
}
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
static int MotorStatus(void *sulf, SConnection *pCon){
|
|
pMotor self = NULL;
|
|
int status;
|
|
|
|
assert(sulf);
|
|
self = (pMotor)sulf;
|
|
|
|
status = evaluateStatus(self,pCon);
|
|
if (self->pDrivInt->drivableStatus!=status) {
|
|
((SConnection *)pCon)->conEventType=STATUS;
|
|
((SConnection *)pCon)->conStatus=status;
|
|
SCWrite(pCon, "", eEvent);
|
|
self->pDrivInt->drivableStatus=status;
|
|
}
|
|
if(status == HWBusy){
|
|
handleMoveCallback(self,pCon);
|
|
}
|
|
return status;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static int MotorZeroCallback(void *userData, void *callData, pHdb node,
|
|
hdbValue v){
|
|
pMotor self = NULL;
|
|
float limit, oldZero, diff;
|
|
SConnection *pCon = NULL;
|
|
|
|
self = (pMotor)userData;
|
|
assert(self != NULL);
|
|
pCon = (SConnection *)callData;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"softzero",&oldZero) == 1);
|
|
|
|
diff = v.v.doubleValue - oldZero;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"softupperlim",&limit) == 1);
|
|
limit -= diff;
|
|
SICSHdbSetFloat(self->pDescriptor->parNode,(SConnection *)callData,
|
|
"softupperlim",limit);
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,pCon,
|
|
"softlowerlim",&limit) == 1);
|
|
limit -= diff;
|
|
SICSHdbSetFloat(self->pDescriptor->parNode,(SConnection *)callData,
|
|
"softlowerlim",limit);
|
|
|
|
UpdateHipadabaPar(node,v,callData);
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int MotorSignCallback(void *userData, void *callData, pHdb node,
|
|
hdbValue v){
|
|
pMotor self = NULL;
|
|
SConnection *pCon = NULL;
|
|
pHdb zero = NULL;
|
|
|
|
self = (pMotor)userData;
|
|
pCon = (SConnection *)callData;
|
|
|
|
if(ABS(v.v.doubleValue - 1.) > .01){
|
|
if(pCon!= NULL){
|
|
SCWrite(pCon,"ERROR: invalid sign value",eError);
|
|
return 0;
|
|
}
|
|
}
|
|
SICSHdbSetFloat(self->pDescriptor->parNode,pCon,
|
|
"softlowerlim",self->pDriver->fLower*v.v.doubleValue);
|
|
SICSHdbSetFloat(self->pDescriptor->parNode,pCon,
|
|
"softupperlim",self->pDriver->fUpper*v.v.doubleValue);
|
|
zero = GetHipadabaNode(self->pDescriptor->parNode,"softzero");
|
|
if(zero != NULL){
|
|
UpdateHipadabaPar(zero,MakeHdbFloat(.0),pCon);
|
|
}
|
|
UpdateHipadabaPar(node,v,callData);
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int MotorDriverReadCallback(void *userData, void *callData, pHdb node,
|
|
hdbValue v){
|
|
pMotor self = NULL;
|
|
SConnection *pCon = NULL;
|
|
int status;
|
|
float value;
|
|
|
|
self = (pMotor)userData;
|
|
assert(self);
|
|
pCon = (SConnection *)callData;
|
|
|
|
if(self->pDriver->GetDriverPar != NULL){
|
|
status = self->pDriver->GetDriverPar(self->pDriver,node->name,
|
|
&value);
|
|
v.dataType = HIPFLOAT;
|
|
v.v.doubleValue = (double)value;
|
|
node->value.v.doubleValue = (double)value;
|
|
return status;
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int MotorDriverSetCallback(void *userData, void *callData, pHdb node,
|
|
hdbValue v){
|
|
pMotor self = NULL;
|
|
SConnection *pCon = NULL;
|
|
int status;
|
|
float value;
|
|
|
|
self = (pMotor)userData;
|
|
assert(self);
|
|
pCon = (SConnection *)callData;
|
|
|
|
if(pCon != NULL && self->pDriver->SetDriverPar != NULL){
|
|
status = self->pDriver->SetDriverPar(self->pDriver,pCon, node->name,
|
|
(float)v.v.doubleValue);
|
|
if(status == 1){
|
|
UpdateHipadabaPar(node,v,callData);
|
|
}
|
|
return status;
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static pHdb MakeMotorDriverNode(pMotor pM, char *name){
|
|
pHdb node = NULL;
|
|
|
|
node = MakeHipadabaNode(name,HIPFLOAT,1);
|
|
if(node == NULL){
|
|
return NULL;
|
|
}
|
|
AppendHipadabaCallback(node,HCBSET,
|
|
MakeHipadabaCallback(MotorDriverSetCallback,pM,NULL,-1,-1));
|
|
AppendHipadabaCallback(node,HCBREAD,
|
|
MakeHipadabaCallback(MotorDriverReadCallback,pM,NULL,-1,-1));
|
|
return node;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int MotorHardCallback(void *userData, void *callData, pHdb node,
|
|
hdbValue v){
|
|
int status;
|
|
pMotor self = NULL;
|
|
float value;
|
|
SConnection *pCon = NULL;
|
|
|
|
self = (pMotor)userData;
|
|
pCon = (SConnection *)callData;
|
|
assert(self != NULL);
|
|
assert(pCon != NULL);
|
|
|
|
status = MotorGetHardPosition(self,pCon,&value);
|
|
node->value.v.doubleValue = (double)value;
|
|
v.v.doubleValue = (double)value;
|
|
return status;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static char *getDriverParList(MotorDriver *pDriv){
|
|
SConnection *pCon = NULL;
|
|
pDynString list = NULL;
|
|
char *listData = NULL;
|
|
|
|
if(pDriv->ListDriverPar != NULL){
|
|
pCon = SCCreateDummyConnection(pServ->pSics);
|
|
if(pCon == NULL){
|
|
return NULL;
|
|
}
|
|
SCStartBuffering(pCon);
|
|
pDriv->ListDriverPar(pDriv,"test.", pCon);
|
|
list = SCEndBuffering(pCon);
|
|
if(list != NULL){
|
|
listData = strdup(GetCharArray(list));
|
|
SCDeleteConnection(pCon);
|
|
} else {
|
|
listData = NULL;
|
|
}
|
|
return listData;
|
|
}
|
|
return NULL;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
extern char *trim(char *str);
|
|
/*--------------------------------------------------------------------------*/
|
|
static char *extractName(char *line){
|
|
char *name = NULL, *pEnd = NULL;
|
|
|
|
name = strchr(line,'.');
|
|
assert(name != NULL);
|
|
while(*name == '.'){
|
|
name++;
|
|
}
|
|
pEnd = strchr(name,'=');
|
|
assert(pEnd != NULL);
|
|
*pEnd = '\0';
|
|
return trim(name);
|
|
}
|
|
/*---------------------------------------------------------------------------
|
|
* This currently uses the ListDriverPar function of the driver. In
|
|
* a later stage this should be modified to test for and use a new
|
|
* driver function, CreateDriverNodes. But this scheme allows to use
|
|
* existing drivers without a change, at the expense of some complicated
|
|
* code.
|
|
* --------------------------------------------------------------------------*/
|
|
static int CreateDriverParameters(pMotor pM, pHdb parent){
|
|
char *listPtr = NULL, line[80], *pPtr, *name;
|
|
pHdb node = NULL;
|
|
|
|
listPtr = getDriverParList(pM->pDriver);
|
|
if(listPtr == NULL){
|
|
/*
|
|
* no driver parameters
|
|
*/
|
|
return 1;
|
|
}
|
|
pPtr = listPtr;
|
|
while((pPtr = stptok(pPtr,line,79,"\n")) != NULL){
|
|
name = extractName(line);
|
|
node = MakeMotorDriverNode(pM, name);
|
|
if(node != NULL){
|
|
AddHipadabaChild(parent,node);
|
|
}
|
|
}
|
|
free(listPtr);
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
pMotor MotorInit(char *drivername, char *name, MotorDriver *pDriv){
|
|
pMotor pM = NULL;
|
|
pHdb node = NULL, child = NULL;
|
|
|
|
assert(drivername);
|
|
assert(pDriv);
|
|
assert(name);
|
|
|
|
/* get memory */
|
|
pM = (pMotor)malloc(sizeof(Motor));
|
|
if(!pM){
|
|
return NULL;
|
|
}
|
|
|
|
/* initialise object descriptor */
|
|
pM->pDescriptor = CreateDescriptor("Motor");
|
|
if(!pM->pDescriptor){
|
|
free(pM);
|
|
return NULL;
|
|
}
|
|
pM->pDescriptor->GetInterface = MotorGetInterface;
|
|
pM->pDescriptor->SaveStatus = MotorSaveStatus;
|
|
|
|
/* copy arguments */
|
|
pM->pDriver = pDriv;
|
|
pM->drivername = strdup(drivername);
|
|
pM->name = strdup(name);
|
|
|
|
|
|
/* create and initialize parameters */
|
|
node = MakeSICSHdbDriv(name,usUser,pM,HIPFLOAT);
|
|
if(node == NULL){
|
|
free(pM);
|
|
return NULL;
|
|
}
|
|
pM->pDescriptor->parNode = node;
|
|
|
|
child = MakeSICSROPar("targetposition",MakeHdbFloat(pM->fPosition));
|
|
AppendHipadabaCallback(child,HCBREAD,
|
|
MakeMemReadCallback(&pM->fTarget));
|
|
AddHipadabaChild(node,child);
|
|
child = MakeSICSROPar("hardposition",MakeHdbFloat(pM->fPosition));
|
|
AppendHipadabaCallback(child,HCBREAD,
|
|
MakeHipadabaCallback(MotorHardCallback,pM,NULL,-1,-1));
|
|
AddHipadabaChild(node,child);
|
|
child = MakeHipadabaNode("sign",HIPFLOAT, 1);
|
|
AppendHipadabaCallback(child,HCBSET, MakeCheckPermissionCallback(usUser));
|
|
AppendHipadabaCallback(child,HCBSET,
|
|
MakeHipadabaCallback(MotorSignCallback,pM,NULL,-1,-1));
|
|
SetHipadabaPar(child,MakeHdbFloat(1.),NULL);
|
|
AddHipadabaChild(node,child);
|
|
child = MakeHipadabaNode("hardlowerlim",HIPFLOAT, 1);
|
|
AppendHipadabaCallback(child,HCBREAD,
|
|
MakeMemReadCallback(&pM->pDriver->fLower));
|
|
AppendHipadabaCallback(child,HCBSET,
|
|
MakeHipadabaCallback(MotorDriverSetCallback,pM,NULL,-1,-1));
|
|
AddHipadabaChild(node,child);
|
|
child = MakeHipadabaNode("hardupperlim",HIPFLOAT, 1);
|
|
AppendHipadabaCallback(child,HCBREAD,
|
|
MakeMemReadCallback(&pM->pDriver->fUpper));
|
|
AppendHipadabaCallback(child,HCBSET,
|
|
MakeHipadabaCallback(MotorDriverSetCallback,pM,NULL,-1,-1));
|
|
AddHipadabaChild(node,child);
|
|
AddHipadabaChild(node,MakeSICSHdbPar("softlowerlim",usUser,
|
|
MakeHdbFloat(pDriv->fLower)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("softupperlim",usUser,
|
|
MakeHdbFloat(pDriv->fUpper)));
|
|
child = MakeHipadabaNode("softzero",HIPFLOAT, 1);
|
|
AppendHipadabaCallback(child,HCBSET, MakeCheckPermissionCallback(usUser));
|
|
AppendHipadabaCallback(child,HCBSET,
|
|
MakeHipadabaCallback(MotorZeroCallback,pM,NULL,-1,-1));
|
|
AddHipadabaChild(node,child);
|
|
AddHipadabaChild(node,MakeSICSHdbPar("fixed",usUser,
|
|
MakeHdbFloat(-1.)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("interruptmode",usMugger,
|
|
MakeHdbFloat(.0)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("precision",usMugger,
|
|
MakeHdbFloat(.1)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("accesscode",usMugger,
|
|
MakeHdbFloat((double)usUser)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("failafter",usMugger,
|
|
MakeHdbFloat((double)3.0)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("maxretry",usMugger,
|
|
MakeHdbFloat((double)3.0)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("ignorefault",usMugger,
|
|
MakeHdbFloat((double).0)));
|
|
AddHipadabaChild(node,MakeSICSHdbPar("movecount",usMugger,
|
|
MakeHdbFloat((double)10.0)));
|
|
pDriv->GetPosition(pDriv,&(pM->fPosition));
|
|
child = MakeSICSROPar("position",MakeHdbFloat(pM->fPosition));
|
|
AppendHipadabaCallback(child,HCBREAD,
|
|
MakeMemReadCallback(&pM->fPosition));
|
|
AddHipadabaChild(node,child);
|
|
|
|
|
|
pM->fTarget = pM->fPosition;
|
|
pM->endScriptID = 0;
|
|
|
|
CreateDriverParameters(pM,pM->pDescriptor->parNode);
|
|
|
|
/* initialise Drivable interface */
|
|
pM->pDrivInt = CreateDrivableInterface();
|
|
if(!pM->pDrivInt){
|
|
DeleteDescriptor(pM->pDescriptor);
|
|
free(pM);
|
|
return NULL;
|
|
}
|
|
pM->pDrivInt->SetValue = MotorRun;
|
|
pM->pDrivInt->CheckLimits = MotorLimits;
|
|
pM->pDrivInt->CheckStatus = MotorStatus;
|
|
pM->pDrivInt->GetValue = MotorGetValue;
|
|
pM->pDrivInt->Halt = MotorHalt;
|
|
|
|
/* initialise callback interface */
|
|
pM->pCall = CreateCallBackInterface();
|
|
if(!pM->pCall){
|
|
MotorKill(pM);
|
|
return NULL;
|
|
}
|
|
|
|
/* done */
|
|
return pM;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
|
|
extern void KillPiPiezo(void *pData);
|
|
|
|
void MotorKill(void *self){
|
|
pMotor pM;
|
|
assert(self);
|
|
|
|
pM = (pMotor)self;
|
|
|
|
/* MotorHalt(pM); */
|
|
|
|
if(pM->name)
|
|
free(pM->name);
|
|
|
|
if(pM->pDrivInt){
|
|
free(pM->pDrivInt);
|
|
}
|
|
|
|
if(pM->pCall){
|
|
DeleteCallBackInterface(pM->pCall);
|
|
}
|
|
|
|
/* kill driver */
|
|
if(pM->drivername){
|
|
if(pM->pDriver->KillPrivate != NULL){
|
|
pM->pDriver->KillPrivate(pM->pDriver);
|
|
}
|
|
if(pM->pDriver->name != NULL){
|
|
free(pM->pDriver->name);
|
|
}
|
|
free(pM->pDriver);
|
|
free(pM->drivername);
|
|
}
|
|
|
|
/* kill Descriptor */
|
|
DeleteDescriptor(pM->pDescriptor);
|
|
|
|
free(pM);
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
int MotorGetPar(pMotor self, char *name, float *fVal){
|
|
char pBueffel[512];
|
|
snprintf(pBueffel,511,"%s ", name);
|
|
return SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
pBueffel, fVal);
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
int MotorSetPar(pMotor self, SConnection *pCon, char *name, float fVal){
|
|
int status;
|
|
|
|
status = SICSHdbSetFloat(self->pDescriptor->parNode,
|
|
pCon,name,fVal);
|
|
if(status == 1){
|
|
SCparChange(pCon);
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
/*---------------------------------------------------------------------------
|
|
MotorCheckBoundary checks for violation of boundary conditions and
|
|
transforms from SoftCoordinates to hard coordinates.
|
|
*/
|
|
|
|
int MotorCheckBoundary(pMotor self, float fVal, float *fNew,
|
|
char *pError, int iErrLen)
|
|
{
|
|
float fHard;
|
|
float fZero, fixed, lowerlim, upperlim, sign;
|
|
char pBueffel[512];
|
|
|
|
assert(self);
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"fixed",&fixed) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"softzero",&fZero) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"softlowerlim",&lowerlim) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"softupperlim",&upperlim) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"sign",&sign) == 1);
|
|
|
|
|
|
/* 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 */
|
|
if(fHard > self->pDriver->fUpper){
|
|
sprintf(pBueffel,"%f violates upper hardware limit %f on %s",
|
|
fVal,self->pDriver->fUpper,self->name);
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0;
|
|
}
|
|
if(fHard < self->pDriver->fLower){
|
|
sprintf(pBueffel,"%f violates lower hardware limit %f on %s",
|
|
fVal,self->pDriver->fLower,self->name);
|
|
strncpy(pError,pBueffel,iErrLen);
|
|
return 0;
|
|
}
|
|
|
|
*fNew = fHard;
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
long MotorRun(void *sulf, SConnection *pCon, float fNew){
|
|
float fHard, rights, ignore, maxerr, interrupt;
|
|
int i, iRet, iCode;
|
|
char pBueffel[512];
|
|
char pError[132];
|
|
pMotor self;
|
|
long lTime;
|
|
float fDelta;
|
|
|
|
self = (pMotor)(sulf);
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"accesscode",&rights) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"ignorefault",&ignore) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"failafter",&maxerr) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"interruptmode",&interrupt) == 1);
|
|
|
|
|
|
/* check if I'am allowed to move this motor */
|
|
if(!SCMatchRights(pCon,(int)rights)){
|
|
sprintf(pBueffel,"ERROR: You are not authorised to move motor %s",
|
|
self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
SCSetInterrupt(pCon,eAbortBatch);
|
|
return 0;
|
|
}
|
|
|
|
/* check boundaries first */
|
|
iRet = MotorCheckBoundary(self,fNew,&fHard,pBueffel,511);
|
|
if(!iRet){
|
|
SCWrite(pCon,pBueffel,eStatus);
|
|
SCSetInterrupt(pCon,eAbortOperation);
|
|
return 0;
|
|
}
|
|
|
|
/* check if the bad motor flag is set */
|
|
if((int)ignore > 0){
|
|
snprintf(pBueffel,511,"WARNING: motor %s is unreliable",
|
|
self->name);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
self->pDrivInt->iErrorCount = 0;
|
|
}
|
|
|
|
/* check our error count and interrupt if to much */
|
|
if(self->pDrivInt->iErrorCount > (int)maxerr){
|
|
/* 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->pDrivInt->iErrorCount = 0;
|
|
return 0;
|
|
}
|
|
|
|
/* Boundaries OK, send command */
|
|
self->posFaultCount = 0;
|
|
self->retryCount = 0;
|
|
self->stopped = 0;
|
|
self->fTarget = fHard;
|
|
self->posCount = 0;
|
|
iRet = self->pDriver->RunTo(self->pDriver,fHard);
|
|
if(iRet != OKOK){
|
|
/* try three times to fix it */
|
|
for(i = 0; (i < 3) && (iRet != OKOK); i++){
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,131);
|
|
sprintf(pBueffel,"WARNING: Trying to fix: %s",pError);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, fHard);
|
|
switch(iRet){
|
|
case MOTFAIL:
|
|
SCWrite(pCon,pError,eError);
|
|
SCWrite(pCon,"\n",eError);
|
|
SCSetInterrupt(pCon,(int)interrupt);
|
|
return HWFault;
|
|
case MOTREDO:
|
|
iRet = self->pDriver->RunTo(self->pDriver,fHard);
|
|
if(iRet == OKOK){
|
|
return OKOK;
|
|
}
|
|
break;
|
|
case MOTOK:
|
|
return OKOK;
|
|
break;
|
|
}
|
|
}
|
|
/* tried three times, refuses to work */
|
|
SCWrite(pCon,pError,eError);
|
|
SCWrite(pCon,"\n",eError);
|
|
SCSetInterrupt(pCon,(int)interrupt);
|
|
return HWFault;
|
|
}
|
|
return OKOK;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
int MotorGetHardPosition(pMotor self,SConnection *pCon, float *fHard){
|
|
int iRet, iCode;
|
|
float fVal, interrupt, sign;
|
|
char pBueffel[512],pError[256];
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"interruptmode",&interrupt) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"sign",&sign) == 1);
|
|
|
|
iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
|
|
if(iRet == OKOK) {
|
|
*fHard = fVal;
|
|
return 1;
|
|
} else {
|
|
/* no point in trying this three times */
|
|
self->pDriver->GetError(self->pDriver,&iCode, pError,255);
|
|
iRet = self->pDriver->TryAndFixIt(self->pDriver,iCode, fVal);
|
|
sprintf(pBueffel,"WARNING: Trying to fix %s",pError);
|
|
SCWrite(pCon,pBueffel,eWarning);
|
|
switch(iRet){
|
|
case MOTFAIL:
|
|
sprintf(pBueffel,"ERROR: cannot fix motor %s",
|
|
self->name);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
SCSetInterrupt(pCon,(int)interrupt);
|
|
*fHard = fVal;
|
|
return 0;
|
|
case MOTOK:
|
|
case MOTREDO:
|
|
iRet = self->pDriver->GetPosition(self->pDriver,&fVal);
|
|
if(iRet){
|
|
*fHard = fVal*sign;
|
|
return 1;
|
|
} else{
|
|
sprintf(pBueffel,"ERROR: cannot fix motor %s",
|
|
self->name);
|
|
SCSetInterrupt(pCon,(int)interrupt);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
*fHard = fVal;
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
*fHard = fVal*sign;
|
|
return 0;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
float MotorHardToSoftPosition(pMotor self, float fValue){
|
|
float sign, zero;
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"sign",&sign) == 1);
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"softzero",&zero) == 1);
|
|
|
|
/* apply zeropoint */
|
|
if(sign < 0.){
|
|
fValue += zero;
|
|
}else {
|
|
fValue -= zero;
|
|
}
|
|
/* apply sign */
|
|
return fValue*sign;
|
|
}
|
|
/* ------------------------------------------------------------------------*/
|
|
int MotorGetSoftPosition(pMotor self, SConnection *pCon, float *fVal){
|
|
int iRet;
|
|
float fValue;
|
|
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
/* get the hard position */
|
|
iRet = MotorGetHardPosition(self,pCon,&fValue);
|
|
if(!iRet){
|
|
*fVal = fValue;
|
|
return 0;
|
|
}
|
|
*fVal = MotorHardToSoftPosition(self,fValue);
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
int MotorCheckPosition(void *sulf, SConnection *pCon){
|
|
float fHard, precision;
|
|
int i, iRet, iCode;
|
|
char pBueffel[512];
|
|
pMotor self;
|
|
|
|
self = (pMotor)sulf;
|
|
assert(self);
|
|
assert(pCon);
|
|
|
|
assert(SICSHdbGetFloat(self->pDescriptor->parNode,NULL,
|
|
"precision",&precision) == 1);
|
|
|
|
/* try to find current position */
|
|
iRet = MotorGetHardPosition(self,pCon,&fHard);
|
|
if(iRet) {
|
|
self->fPosition = fHard;
|
|
if(ABS(fHard - self->fTarget) < precision){
|
|
return 1;
|
|
} else {
|
|
/* Oooopppsss error */
|
|
return 0;
|
|
}
|
|
} else {
|
|
/* error getting hold of position, MotorGetHard already tried to
|
|
solve the problem and FAILED, client already knows...*/
|
|
return -1;
|
|
}
|
|
}
|
|
/* --------------------------------------------------------------------------
|
|
The Factory function for creating a motor. Usage:
|
|
MotorCreate name DriverName.
|
|
|
|
Drivernames currently understood are:
|
|
EL734 SINQ EL734 stepper motor
|
|
EL734DC SINQ EL734 DC motor
|
|
PiPiezo Physik-Instrumente E-255 Piezo Controller
|
|
SIM simulated motor
|
|
*/
|
|
|
|
extern MotorDriver *MakePiPiezo(Tcl_Interp *pTcl, char *pArray);
|
|
|
|
int MotorCreate(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pMotor pNew = NULL;
|
|
MotorDriver *pDriver = NULL;
|
|
char pBueffel[512];
|
|
int iD, iRet;
|
|
Tcl_Interp *pTcl = (Tcl_Interp *)pSics->pTcl;
|
|
pSite site = NULL;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
|
|
/* a first check */
|
|
if(argc < 3)
|
|
{
|
|
SCWrite(pCon,"Insufficient arguments for motor creation",eError);
|
|
return 0;
|
|
}
|
|
|
|
/* create the driver */
|
|
strtolower(argv[2]);
|
|
strtolower(argv[1]);
|
|
if (strcmp(argv[2],"sim") == 0)
|
|
{
|
|
iD = argc - 3;
|
|
pDriver = CreateSIM(pCon,iD,&argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("SIM",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}else if (strcmp(argv[2],"tclmot") == 0)
|
|
{
|
|
pDriver = CreateTclMotDriv(pCon,argc,argv);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("TCLMOT",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
} else if(strcmp(argv[2],"regress") == 0)
|
|
{
|
|
pDriver = RGMakeMotorDriver();
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("regress",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
site = getSite();
|
|
if(site != NULL)
|
|
{
|
|
pNew = site->CreateMotor(pCon,argc-1,&argv[1]);
|
|
}
|
|
if(pNew == NULL)
|
|
{
|
|
sprintf(pBueffel,"Motor Type %s not recognized for motor %s",
|
|
argv[2],argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/* create the interpreter command */
|
|
iRet = AddCommand(pSics,argv[1],MotorAction,MotorKill,pNew);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/* ----------------- some private functions used in MotorAction -------------*/
|
|
void MotorListLL(pMotor self, SConnection *pCon)
|
|
{
|
|
char pBueffel[256];
|
|
|
|
snprintf(pBueffel,255,"Parameter Listing for motor %s",self->name);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
snprintf(pBueffel,255,"%s ",self->name);
|
|
PrintSICSParList(self->pDescriptor->parNode,pCon,pBueffel);
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
void MotorReset(pMotor pM)
|
|
{
|
|
SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"softlowerlim",
|
|
pM->pDriver->fLower);
|
|
SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"softupperlim",
|
|
pM->pDriver->fUpper);
|
|
SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"softzero",
|
|
.0);
|
|
SICSHdbSetFloat(pM->pDescriptor->parNode,NULL,"fixed",
|
|
-1.);
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
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);
|
|
}
|
|
free(self);
|
|
}
|
|
/*------------------- The CallBack function for interest ------------------*/
|
|
static int InterestCallback(int iEvent, void *pEvent, void *pUser,
|
|
commandContext cc)
|
|
{
|
|
pMotInfo pInfo = NULL;
|
|
char pBueffel[80];
|
|
MotCallback *psCall;
|
|
|
|
assert(pEvent);
|
|
assert(pUser);
|
|
|
|
psCall = (MotCallback *)pEvent;
|
|
pInfo = (MotInfo *)pUser;
|
|
|
|
if (pInfo->lastValue != psCall->fVal) {
|
|
pInfo->lastValue = psCall->fVal;
|
|
(pInfo->pCon)->conEventType=POSITION;
|
|
sprintf(pBueffel,"%s.position = %f ", pInfo->pName, pInfo->lastValue);
|
|
SCWriteInContext(pInfo->pCon,pBueffel,eEvent,cc);
|
|
}
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static void KillScript(void *pData)
|
|
{
|
|
if(pData != NULL)
|
|
{
|
|
free(pData);
|
|
}
|
|
}
|
|
/*------------------------ The endscript callback function ----------------*/
|
|
static int EndScriptCallback(int iEvent, void *pEvent, void *pUser,
|
|
commandContext cc)
|
|
{
|
|
char *pScript = NULL;
|
|
MotCallback *psCall;
|
|
char pCommand[1024];
|
|
int iRet;
|
|
|
|
assert(pEvent);
|
|
assert(pUser);
|
|
|
|
psCall = (MotCallback *)pEvent;
|
|
pScript = (char *)pUser;
|
|
|
|
sprintf(pCommand,"%s %f",pScript,psCall->fVal);
|
|
iRet = Tcl_Eval(pServ->pSics->pTcl,pCommand);
|
|
return iRet;
|
|
}
|
|
/*----------------------------------------------------------------------------
|
|
The wrapper function for a motor. Commands currently supported are:
|
|
|
|
motorname parametername newval : sets a parameter
|
|
motorname parametername : gets the value for a par
|
|
motorname list : lists all parameters
|
|
motorname reset : puts softlimits to default
|
|
motorname interest : starts sending automatic
|
|
notifications when driving
|
|
motorname endscript : script to call when motor
|
|
finishes driving
|
|
-----------------------------------------------------------------------------*/
|
|
int MotorAction(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
char pBueffel[512];
|
|
TokenList *pList = NULL;
|
|
TokenList *pCurrent;
|
|
TokenList *pName;
|
|
int iRet;
|
|
pMotor self;
|
|
float fValue;
|
|
long lID;
|
|
pMotInfo pMoti = NULL;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
assert(pData);
|
|
|
|
self = (pMotor)pData;
|
|
|
|
/* create Tokenlist */
|
|
argtolower(argc,argv);
|
|
pList = SplitArguments(argc,argv);
|
|
if(!pList)
|
|
{
|
|
SCWrite(pCon,"Error parsing argument list in MotorAction",eError);
|
|
return 0;
|
|
}
|
|
|
|
/* first argument can be one of list, reset or parameter name */
|
|
pCurrent = pList->pNext;
|
|
if(!pCurrent) /* no argument, print value */
|
|
{
|
|
iRet = MotorGetSoftPosition(self,pCon,&fValue);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"Error obtaining position for %s",argv[0]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
DeleteTokenList(pList);
|
|
return 0;
|
|
}
|
|
sprintf(pBueffel,"%s = %f",argv[0],fValue);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
DeleteTokenList(pList);
|
|
return 1;
|
|
}
|
|
|
|
/* check for list */
|
|
if(strcmp(pCurrent->text,"list") == 0)
|
|
{
|
|
MotorListLL(self,pCon);
|
|
DeleteTokenList(pList);
|
|
return 1;
|
|
} /* check for reset */
|
|
else if(strcmp(pCurrent->text,"reset") == 0)
|
|
{
|
|
if(!SCMatchRights(pCon,usUser))
|
|
{
|
|
sprintf(pBueffel,"Insufficient privilege to reset %s",
|
|
argv[0]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
DeleteTokenList(pList);
|
|
return 0;
|
|
}
|
|
MotorReset(self);
|
|
DeleteTokenList(pList);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
else if(strcmp(pCurrent->text,"interest") == 0) /* interest */
|
|
{
|
|
pMoti = (pMotInfo)malloc(sizeof(MotInfo));
|
|
if(!pMoti)
|
|
{
|
|
SCWrite(pCon,"ERROR: out of memory in motor.c",eError);
|
|
return 0;
|
|
}
|
|
pMoti->pName = strdup(argv[0]);
|
|
pMoti->pCon = pCon;
|
|
iRet = MotorGetSoftPosition(self,pCon,&fValue);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"Failed to register interest, Reason:Error obtaining current position for %s",argv[0]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
DeleteTokenList(pList);
|
|
return 0;
|
|
}
|
|
pMoti->lastValue = fValue;
|
|
|
|
lID = RegisterCallback(self->pCall, SCGetContext(pCon),MOTDRIVE, InterestCallback,
|
|
pMoti, KillInfo);
|
|
SCRegister(pCon,pSics, self->pCall,lID);
|
|
DeleteTokenList(pList);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
else if(strcmp(pCurrent->text,"uninterest") == 0)
|
|
{
|
|
RemoveCallback2(self->pCall,pCon);
|
|
SCSendOK(pCon);
|
|
DeleteTokenList(pList);
|
|
return 1;
|
|
}
|
|
else if(strcmp(pCurrent->text,"endscript") == 0) /* endscript */
|
|
{
|
|
if(!SCMatchRights(pCon,usMugger))
|
|
{
|
|
return 0;
|
|
}
|
|
if(self->endScriptID != 0)
|
|
{
|
|
RemoveCallback(self->pCall, self->endScriptID);
|
|
self->endScriptID = 0;
|
|
}
|
|
pCurrent = pCurrent->pNext;
|
|
if(!pCurrent)
|
|
{
|
|
SCWrite(pCon,"ERROR: scriptname argument missing",eError);
|
|
return 0;
|
|
}
|
|
self->endScriptID =
|
|
RegisterCallback(self->pCall, SCGetContext(pCon),MOTEND, EndScriptCallback,
|
|
strdup(pCurrent->text), KillScript);
|
|
SCRegister(pCon,pSics, self->pCall,self->endScriptID);
|
|
DeleteTokenList(pList);
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
else /* one of the parameter commands or error left now */
|
|
{
|
|
snprintf(pBueffel,511,"%s ", argv[0]);
|
|
iRet = ProcessSICSHdbPar(self->pDescriptor->parNode,
|
|
pCon, pBueffel, argc-1, &argv[1]);
|
|
if(iRet == -1) {
|
|
SCWrite(pCon,"ERROR: subcommand to motor not understood",
|
|
eError);
|
|
return 0;
|
|
} else if(iRet < -1) {
|
|
return 0;
|
|
}
|
|
return iRet;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|