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

724 lines
20 KiB
C
Raw Permalink Blame History

/*-----------------------------------------------------------------------
A configurable virtual motor object. At motor start a script is called
which returns the motors and positions to drive as a comma separated
list holding entries of the form mot=value.
This is the implementation file.
COPYRIGHT: see file COPYRIGHT
Mark Koennecke, August 2004
interest added: Ferdi Franceschini, April 2006
Additions:
- a checkscript to be run at the end of driving the motor
- a means to call the write script multiple times. This is useful if
you need to drive to the value in multiple steps. An example is the
BOA double crystal monochromator where you first have to put the blades to
0, then drive the translation and then drive the blades to the real theta.
All this to avoid collisions. In order to support this, a state field and
and a readable target field has been added. The mode of operation is such
that the on first run the, writescript set the state to something different
then idle. This causes after the first set of motors finished running the
writescript to be called again. This can figure out by lookin at the
state variable what to do. This can be done in repetition until the
writescript sets state to idle again.
Mark Koennecke, April 2012
-------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include "fortify.h"
#include <tcl.h>
#include "lld.h"
#include "splitter.h"
#include "confvirtmot.h"
#include "confvirtmot.i"
#define BAD_VALUE (-9999.99)
extern char *stptok(char *s, char *t, int len, char *brk);
extern int CheckMotiMatch(const void* context, const void* pUserData);
extern double DoubleTime(void);
/*---------------------------------------------------------------------------
An internal data structure which holds information required to control
the real motors
----------------------------------------------------------------------------*/
typedef struct {
char name[132];
void *data;
pIDrivable pDriv;
float value;
int running;
long taskID;
} RealMotor, *pRealMotor;
/* Data passed by event generating object */
typedef struct {
float fVal;
char *pName;
} EventInfo;
/* Data available when registering interest */
typedef struct {
char *pName;
SConnection *pCon;
float lastValue;
} RegisteredInfo, *pRegisteredInfo;
/*---------------------------------------------------------------------*/
static int HaltMotors(void *pData)
{
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
RealMotor tuktuk;
int iRet;
iRet = LLDnodePtr2First(self->motorList);
while (iRet != 0) {
LLDnodeDataTo(self->motorList, &tuktuk);
tuktuk.pDriv->Halt(tuktuk.data);
iRet = LLDnodePtr2Next(self->motorList);
}
return 1;
}
/*----------------------------------------------------------------------*/
static char *invokeMotorScript(pConfigurableVirtualMotor self,
float newValue)
{
char pBueffel[512];
Tcl_Interp *pTcl;
int status;
self->parseOK = 1;
if (self->scriptName == NULL) {
snprintf(self->scriptError, 511,
"ERROR: no script configured for configurable virtual motor");
self->parseOK = 0;
return NULL;
}
snprintf(pBueffel, 510, "%s %f", self->scriptName, newValue);
pTcl = InterpGetTcl(pServ->pSics);
status = Tcl_Eval(pTcl, pBueffel);
if (status != TCL_OK) {
snprintf(self->scriptError, 510, "ERROR: Tcl subsystem reported %s",
Tcl_GetStringResult(pTcl));
self->parseOK = 0;
return NULL;
}
return (char *) Tcl_GetStringResult(pTcl);
}
/*--------------------------------------------------------------------*/
static int parseEntry(pConfigurableVirtualMotor self, char *entry)
{
RealMotor tuktuk;
char *pPtr = NULL;
char number[80], pError[256];
CommandList *pCom = NULL;
int status;
pPtr = entry;
pPtr = stptok(pPtr, tuktuk.name, 131, "=");
if (pPtr == NULL) {
snprintf(self->scriptError, 510, "ERROR: cannot interpret %s, %s",
entry, "require format: motor=value");
self->parseOK = 0;
return 0;
}
tuktuk.pDriv = (pIDrivable) FindDrivable(pServ->pSics, tuktuk.name);
pCom = FindCommand(pServ->pSics, tuktuk.name);
if (!pCom) {
snprintf(self->scriptError, 510, "ERROR: %s not found", tuktuk.name);
self->parseOK = 0;
return 0;
}
tuktuk.data = pCom->pData;
if (tuktuk.pDriv == NULL) {
snprintf(self->scriptError, 510, "ERROR: %s is not drivable",
tuktuk.name);
self->parseOK = 0;
return 0;
}
pPtr = stptok(pPtr, number, 79, "=");
tuktuk.value = atof(number);
LLDnodeAppendFrom(self->motorList, &tuktuk);
return 1;
}
/*----------------------------------------------------------------------*/
static int parseCommandList(pConfigurableVirtualMotor self,
char *commandList)
{
char pEntry[256], *pPtr;
int status;
LLDdelete(self->motorList);
self->motorList = LLDcreate(sizeof(RealMotor));
pPtr = commandList;
while ((pPtr = stptok(pPtr, pEntry, 255, ",")) != NULL) {
status = parseEntry(self, pEntry);
if (status != 1) {
return 0;
}
}
return 1;
}
/*-----------------------------------------------------------------------*/
static int startMotorList(pConfigurableVirtualMotor self,
SConnection * pCon)
{
RealMotor tuktuk;
int status, iRet;
iRet = LLDnodePtr2First(self->motorList);
while (iRet != 0) {
LLDnodeDataTo(self->motorList, &tuktuk);
tuktuk.taskID = StartDriveTask(tuktuk.data,pCon,tuktuk.name,tuktuk.value);
if (tuktuk.taskID < 0) {
return HWFault;
}
tuktuk.running = 1;
LLDnodeDataFrom(self->motorList, &tuktuk);
iRet = LLDnodePtr2Next(self->motorList);
}
return OKOK;
}
/*------------------------------------------------------------------------*/
static long ConfSetValue(void *pData, SConnection * pCon, float newValue)
{
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
char *commandList = NULL;
int status;
assert(self != NULL);
commandList = invokeMotorScript(self, newValue);
if (commandList == NULL) {
SCWrite(pCon, self->scriptError, eError);
return 0;
}
status = parseCommandList(self, commandList);
if (status != 1) {
SCWrite(pCon, self->scriptError, eError);
return 0;
}
self->targetValue = newValue;
self->targetReached = 0;
self->posCount = 0;
self->last_report_time = 0.0;
status = startMotorList(self, pCon);
if (status != OKOK) {
HaltMotors(self);
}
return OKOK;
}
/*-----------------------------------------------------------------------*/
static int ConfCheckLimits(void *pData, float fVal, char *error,
int errLen)
{
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
char *commandList = NULL;
int status, iRet;
RealMotor tuktuk;
assert(self != NULL);
if (self->targetValue != fVal) {
commandList = invokeMotorScript(self, fVal);
if (commandList == NULL) {
strlcpy(error, self->scriptError, errLen);
return 0;
}
status = parseCommandList(self, commandList);
if (status != 1) {
strlcpy(error, self->scriptError, errLen);
return 0;
}
}
if(self->state != NULL){
free(self->state);
}
self->state = strdup("idle");
iRet = LLDnodePtr2First(self->motorList);
while (iRet != 0) {
LLDnodeDataTo(self->motorList, &tuktuk);
status =
tuktuk.pDriv->CheckLimits(tuktuk.data, tuktuk.value, error,
errLen);
if (status != 1) {
return status;
}
iRet = LLDnodePtr2Next(self->motorList);
}
return 1;
}
static void KillInfo(void *pData)
{
pRegisteredInfo self = NULL;
assert(pData);
self = (pRegisteredInfo) pData;
if (self->pName) {
free(self->pName);
}
if (self->pCon) {
SCDeleteConnection(self->pCon);
}
free(self);
}
/*------------------- The CallBack function for interest ------------------
* iEvent: Event ID, see event.h for SICS events
* pEvent: May contain data from event generating object
* pUser: Data available when registering interest, see RegisteredInfo struct
* defined above for available info
--------------------------------------------------------------------------*/
static int InterestCallback(int iEvent, void *pEvent, void *pUser)
{
pRegisteredInfo pInfo = NULL;
char pBueffel[80];
EventInfo *pEventData;
assert(pEvent);
assert(pUser);
pEventData = (EventInfo *) pEvent;
pInfo = (RegisteredInfo *) pUser;
if (!SCisConnected(pInfo->pCon)) {
return -1;
}
if (pInfo->lastValue != pEventData->fVal) {
pInfo->lastValue = pEventData->fVal;
(pInfo->pCon)->conEventType = POSITION;
sprintf(pBueffel, "%s.position = %f ", pInfo->pName, pInfo->lastValue);
SCWrite(pInfo->pCon, pBueffel, eEvent);
}
return 1;
}
/*---------------------------------------------------------------------*/
static void invokeCheckScript(pConfigurableVirtualMotor self,
SConnection * pCon)
{
Tcl_Interp *pTcl;
int status;
pTcl = InterpGetTcl(pServ->pSics);
if(self->checkScript != NULL){
status = Tcl_Eval(pTcl, self->checkScript);
if (status != TCL_OK) {
snprintf(self->scriptError, 510, "ERROR: Tcl subsystem reported %s",
Tcl_GetStringResult(pTcl));
SCWrite(pCon, self->scriptError, eError);
}
}
}
/*------------------------------------------------------------------------*/
static int ConfCheckStatus(void *pData, SConnection * pCon)
{
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
RealMotor tuktuk;
int iRet, status, result;
EventInfo event;
result = HWIdle;
iRet = LLDnodePtr2First(self->motorList);
while (iRet != 0) {
LLDnodeDataTo(self->motorList, &tuktuk);
if (tuktuk.running == 1) {
status = isTaskIDRunning(pServ->pTasker,tuktuk.taskID);
if(status == 1){
return HWBusy;
} else {
tuktuk.running = 0;
LLDnodeDataFrom(self->motorList, &tuktuk);
}
}
iRet = LLDnodePtr2Next(self->motorList);
}
if (result == HWIdle) {
/*
* Do we have to do another step? If so run it.
*/
if(strstr(self->state,"idle") == NULL){
status = ConfSetValue(self,pCon,self->targetValue);
if(status == OKOK){
result = HWBusy;
}
} else {
invokeCheckScript(self,pCon);
event.pName = self->name;
event.fVal = self->pDriv->GetValue(self, pCon);
InvokeCallBack(self->pCall, MOTDRIVE, &event);
}
} else if (result == HWBusy) {
double current_time, skip_time;
current_time = DoubleTime();
skip_time = 0.500;
if (self->last_report_time + skip_time <= current_time) {
event.pName = self->name;
event.fVal = self->pDriv->GetValue(self, pCon);
InvokeCallBack(self->pCall, MOTDRIVE, &event);
self->last_report_time = current_time;
}
}
return result;
}
/*---------------------------------------------------------------------*/
static float invokeReadScript(pConfigurableVirtualMotor self,
SConnection * pCon)
{
Tcl_Interp *pTcl;
int status;
pTcl = InterpGetTcl(pServ->pSics);
status = Tcl_Eval(pTcl, self->readScript);
if (status != TCL_OK) {
snprintf(self->scriptError, 510, "ERROR: Tcl subsystem reported %s",
Tcl_GetStringResult(pTcl));
SCWrite(pCon, self->scriptError, eError);
}
return atof(Tcl_GetStringResult(pTcl));
}
/*---------------------------------------------------------------------*/
static void checkMotorValues(pConfigurableVirtualMotor self,
SConnection * pCon)
{
int iRet;
float value;
RealMotor tuktuk;
char pBueffel[512];
iRet = LLDnodePtr2First(self->motorList);
while (iRet != 0) {
LLDnodeDataTo(self->motorList, &tuktuk);
value = tuktuk.pDriv->GetValue(tuktuk.data, pCon);
value -= tuktuk.value;
if (value < .0) {
value *= -1;
}
if (value > .1) {
snprintf(pBueffel, 510, "WARNING: motor %s of position by %f",
tuktuk.name, value);
SCWrite(pCon, pBueffel, eWarning);
return;
}
iRet = LLDnodePtr2Next(self->motorList);
}
self->targetReached = 1;
}
/*-----------------------------------------------------------------------*/
static float ConfGetValue(void *pData, SConnection * pCon)
{
pConfigurableVirtualMotor self = (pConfigurableVirtualMotor) pData;
float currentValue = BAD_VALUE;
assert(self != NULL);
if (self->readScript != NULL) {
currentValue = invokeReadScript(self, pCon);
} else {
if (self->targetReached == 1) {
return self->targetValue;
} else {
checkMotorValues(self, pCon);
if (self->targetReached) {
currentValue = self->targetValue;
}
}
}
return currentValue;
}
/*-----------------------------------------------------------------------*/
static void *GetConfigurableVirtualMotorInterface(void *pData, int iID)
{
pConfigurableVirtualMotor self = NULL;
self = (pConfigurableVirtualMotor) pData;
assert(self);
if (iID == DRIVEID) {
return self->pDriv;
} else if (iID == CALLBACKINTERFACE) {
return self->pCall;
}
return NULL;
}
/*-----------------------------------------------------------------------*/
static void KillConfigurableVirtualMotor(void *data)
{
pConfigurableVirtualMotor self = NULL;
self = (pConfigurableVirtualMotor) data;
if (self != NULL) {
if (self->pDes != NULL) {
DeleteDescriptor(self->pDes);
self->pDes = NULL;
}
LLDdelete(self->motorList);
if (self->name != NULL) {
free(self->name);
self->name = NULL;
}
if (self->scriptName != NULL) {
free(self->scriptName);
self->scriptName = NULL;
}
if (self->readScript != NULL) {
free(self->readScript);
self->readScript = NULL;
}
if(self->state != NULL){
free(self->state);
self->state = NULL;
}
if (self->pCall != NULL) {
DeleteCallBackInterface(self->pCall);
self->pCall = NULL;
}
if (self->pDriv != NULL) {
free(self->pDriv);
self->pDriv = NULL;
}
free(self);
self = NULL;
}
}
/*------------------------------------------------------------------------*/
int MakeConfigurableVirtualMotor(SConnection * pCon, SicsInterp * pSics,
void *data, int argc, char *argv[])
{
pConfigurableVirtualMotor pNew = NULL;
char pBueffel[512];
if (argc < 2) {
SCWrite(pCon, "ERROR: need name to create configurable motor", eError);
return 0;
}
pNew =
(pConfigurableVirtualMotor) malloc(sizeof(ConfigurableVirtualMotor));
if (pNew == NULL) {
SCWrite(pCon, "ERROR: no memory to create configurable virtual motor",
eError);
return 0;
}
memset(pNew, 0, sizeof(ConfigurableVirtualMotor));
pNew->name = strdup(argv[1]);
pNew->posCount = 0;
pNew->last_report_time = 0.0;
pNew->pDes = CreateDescriptor("ConfigurableVirtualMotor");
pNew->pDriv = CreateDrivableInterface();
pNew->motorList = LLDcreate(sizeof(RealMotor));
if (pNew->pDes == NULL || pNew->pDriv == NULL || pNew->motorList < 0) {
SCWrite(pCon, "ERROR: no memory to create configurable virtual motor",
eError);
return 0;
}
pNew->state = strdup("idle");
/*
assign functions
*/
pNew->pDes->GetInterface = GetConfigurableVirtualMotorInterface;
pNew->pDriv->Halt = HaltMotors;
pNew->pDriv->CheckLimits = ConfCheckLimits;
pNew->pDriv->SetValue = ConfSetValue;
pNew->pDriv->CheckStatus = ConfCheckStatus;
pNew->pDriv->GetValue = ConfGetValue;
/* initialise callback interface */
pNew->pCall = CreateCallBackInterface();
if (!pNew->pCall) {
KillConfigurableVirtualMotor(pNew);
return 0;
}
/*
install command
*/
if (AddCommand(pSics, argv[1], ConfigurableVirtualMotorAction,
KillConfigurableVirtualMotor, pNew) != 1) {
snprintf(pBueffel, 510, "ERROR: duplicate command %s not created",
argv[1]);
SCWrite(pCon, pBueffel, eError);
return 0;
} else {
SCSendOK(pCon);
}
return 1;
}
/*-----------------------------------------------------------------------*/
int ConfigurableVirtualMotorAction(SConnection * pCon, SicsInterp * pSics,
void *data, int argc, char *argv[])
{
pConfigurableVirtualMotor self = NULL;
char pBueffel[512];
float value;
long lID;
pRegisteredInfo pRegInfo = NULL;
self = (pConfigurableVirtualMotor) data;
assert(self != NULL);
if (argc > 1) {
strtolower(argv[1]);
if (strcmp(argv[1], "drivescript") == 0) {
if (argc > 2) {
/* set case */
Arg2Text(argc - 2, &argv[2], pBueffel, 510);
self->scriptName = strdup(pBueffel);
SCSendOK(pCon);
return 1;
} else {
/* inquiry */
if (self->scriptName == NULL) {
snprintf(pBueffel, 510, "%s.drivescript = UNDEFINED", argv[0]);
SCWrite(pCon, pBueffel, eValue);
return 1;
} else {
snprintf(pBueffel, 510, "%s.drivescript = %s", argv[0],
self->scriptName);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
} else if (strcmp(argv[1], "readscript") == 0) {
if (argc > 2) {
/* set case */
Arg2Text(argc - 2, &argv[2], pBueffel, 510);
self->readScript = strdup(pBueffel);
SCSendOK(pCon);
return 1;
} else {
/* inquiry */
if (self->readScript == NULL) {
snprintf(pBueffel, 510, "%s.readscript = UNDEFINED", argv[0]);
SCWrite(pCon, pBueffel, eValue);
return 1;
} else {
snprintf(pBueffel, 510, "%s.readscript = %s", argv[0],
self->readScript);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
} else if (strcmp(argv[1], "checkscript") == 0) {
if (argc > 2) {
/* set case */
Arg2Text(argc - 2, &argv[2], pBueffel, 510);
self->checkScript = strdup(pBueffel);
SCSendOK(pCon);
return 1;
} else {
/* inquiry */
if (self->checkScript == NULL) {
snprintf(pBueffel, 510, "%s.checkscript = UNDEFINED", argv[0]);
SCWrite(pCon, pBueffel, eValue);
return 1;
} else {
snprintf(pBueffel, 510, "%s.checkscript = %s", argv[0],
self->checkScript);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
} else if (strcmp(argv[1], "state") == 0) {
if (argc > 2) {
/* set case */
Arg2Text(argc - 2, &argv[2], pBueffel, 510);
if(self->state != NULL){
free(self->state);
}
self->state = strdup(pBueffel);
SCSendOK(pCon);
return 1;
} else {
/* inquiry */
if (self->state == NULL) {
snprintf(pBueffel, 510, "%s.state = UNDEFINED", argv[0]);
SCWrite(pCon, pBueffel, eValue);
return 1;
} else {
snprintf(pBueffel, 510, "%s.state = %s", argv[0],
self->state);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
} else if (strcmp(argv[1], "target") == 0) {
/* inquiry */
snprintf(pBueffel, 510, "%s.target = %f", argv[0], self->targetValue);
SCWrite(pCon, pBueffel, eValue);
return 1;
} else if (strcmp(argv[1], "interest") == 0) {
pRegInfo = (pRegisteredInfo) malloc(sizeof(RegisteredInfo));
if (!pRegInfo) {
SCWrite(pCon, "ERROR: out of memory in confvirtualmot.c", eError);
return 0;
}
pRegInfo->pName = strdup(argv[0]);
pRegInfo->pCon = SCCopyConnection(pCon);
value = ConfGetValue(self, pCon);
if (value < -9990.) {
snprintf(pBueffel,511,
"Failed to register interest, Reason:Error obtaining current position for %s",
argv[0]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
pRegInfo->lastValue = value;
RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon); /* only this one */
lID = RegisterCallback(self->pCall, MOTDRIVE,
InterestCallback, pRegInfo, KillInfo);
SCSendOK(pCon);
return 1;
} else if(strcmp(argv[1],"uninterest") == 0) {
RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon);
SCSendOK(pCon);
return 1;
} else {
snprintf(pBueffel, sizeof(pBueffel), "ERROR: subcommand %s to %s unknown",
argv[1], argv[0]);
SCWrite(pCon, pBueffel, eError);
return 0;
}
} else {
value = self->pDriv->GetValue(self, pCon);
snprintf(pBueffel, 510, "%s = %f", argv[0], value);
SCWrite(pCon, pBueffel, eValue);
return 1;
}
}
Reference in New Issue View Git Blame Copy Permalink