283 lines
8.1 KiB
C
283 lines
8.1 KiB
C
/**
|
|
* --------------------------------------------------------------*/
|
|
#include <sics.h>
|
|
#include <sicshipadaba.h>
|
|
#include <devser.h>
|
|
#include <tcl.h>
|
|
#include <macro.h>
|
|
#include <sicsobj.h>
|
|
#include "scriptcontext.h"
|
|
/*---------------------------------------------------------------*/
|
|
typedef struct {
|
|
pObjectDescriptor pDes;
|
|
pIDrivable pDrivInt; /* Need a drivable interface for "emon list" */
|
|
pEVInterface pEVI;
|
|
EVMode eMode;
|
|
int iPaused;
|
|
char *pName;
|
|
pHdb modeNode, tolNode, errNode;
|
|
}SctEmon, *pSctEmon;
|
|
/* emon will monitor when mode is "monitor" or "paused" */
|
|
/* XXX nopause
|
|
static char *modestr[5] = {"idle", "drive", "monitor", "error", "paused"};
|
|
static int EVPaused=4;*/
|
|
/*---------------------------------------------------------------*/
|
|
/**
|
|
@brief a dummy getvalue command to prevent "emon list" from aborting SICS
|
|
*/
|
|
static float SCTEmonGetValue(void *pData, SConnection *pCon)
|
|
{
|
|
return -999.666;
|
|
}
|
|
/* XXX nopause
|
|
static void SetModeNode(pSctEmon self, EVMode mode)
|
|
{
|
|
if (self->modeNode->value.v.text != NULL)
|
|
free(self->modeNode->value.v.text);
|
|
self->modeNode->value.v.text = strdup(modestr[mode]);
|
|
}
|
|
*/
|
|
|
|
static void *SCTEmonGetInterface(void *pData, int ID){
|
|
pSctEmon self = NULL;
|
|
|
|
self = (pSctEmon)pData;
|
|
assert(self);
|
|
if (self->modeNode == NULL || self->tolNode == NULL || self->errNode == NULL)
|
|
return NULL;
|
|
|
|
switch (ID) {
|
|
case DRIVEID:
|
|
return self->pDrivInt;
|
|
case ENVIRINTERFACE:
|
|
return self->pEVI;
|
|
default:
|
|
return NULL;
|
|
}
|
|
}
|
|
/*----------------------------------------------------------------
|
|
Reads node value and returns EVMode
|
|
Convert mode string from the evmode textnode value to
|
|
typedef enum { EVIdle, EVDrive, EVMonitor, EVError } EVMode;
|
|
"idle", "drive", "monitor", "error"
|
|
------------------------------------------------------------------*/
|
|
static EVMode SCTEmonGetMode(void *pData)
|
|
{
|
|
pSctEmon self = NULL;
|
|
|
|
self = (pSctEmon)pData;
|
|
assert(self);
|
|
switch(self->modeNode->value.v.text[0]) {
|
|
case 'I':
|
|
case 'i':
|
|
return EVIdle;
|
|
break;
|
|
case 'D':
|
|
case 'd':
|
|
return EVDrive;
|
|
break;
|
|
case 'M':
|
|
case 'm':
|
|
/*XXX nopause case 'P':
|
|
case 'p':*/
|
|
return EVMonitor;
|
|
break;
|
|
case 'E':
|
|
case 'e':
|
|
return EVError;
|
|
break;
|
|
default:
|
|
return EVIdle;
|
|
//TODO Handle error
|
|
}
|
|
}
|
|
/*----------------------------------------------------------------
|
|
This routine can return 0 when a limit problem occurred
|
|
OKOK when the motor was successfully started
|
|
HWFault when a problem occured starting the device
|
|
Possible errors shall be printed to pCon
|
|
For real motors, this is supposed to try at least three times
|
|
to start the motor in question
|
|
val is the value to drive the motor too
|
|
Reads value from the "isintolerance" node (1 or 0)
|
|
------------------------------------------------------------------*/
|
|
static int SCTEmonIsInTolerance(void *pData)
|
|
{
|
|
pSctEmon self = NULL;
|
|
pExeList pExe;
|
|
char pBueffel[512];
|
|
char monMode;
|
|
|
|
self = (pSctEmon)pData;
|
|
assert(self);
|
|
|
|
if(self->tolNode->value.v.intValue == 1) {
|
|
monMode = self->modeNode->value.v.text[0];
|
|
if (self->iPaused) {
|
|
pExe = GetExecutor();
|
|
ContinueExecution(pExe);
|
|
self->iPaused = 0;
|
|
sprintf(pBueffel,"Device %s back in tolerances again", self->pName);
|
|
SCWrite(GetExeOwner(pExe), pBueffel, eWarning);
|
|
}
|
|
//XXX nopause SetModeNode(self, EVMonitor);
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
/*----------------------------------------------------------------
|
|
Checks the status of a running motor. Possible return values
|
|
HWBusy The motor is still running
|
|
OKOK or HWIdle when the motor finished driving
|
|
HWFault when a hardware problem ocurred
|
|
HWPosFault when the hardware cannot reach a position
|
|
Errors are duly to be printed to pCon
|
|
For real motors CheckStatus again shall try hard to fix any
|
|
issues with the motor
|
|
|
|
The HandleError node value can be one of Lazy, Pause, Interrupt, Safe
|
|
NOTE: emon ignores the returned iStatus value
|
|
------------------------------------------------------------------*/
|
|
static int SCTEmonHandleError(void *pData)
|
|
{
|
|
pSctEmon self = NULL;
|
|
pExeList pExe;
|
|
int iRet, iHandler, iStatus;
|
|
char monMode;
|
|
|
|
self = (pSctEmon)pData;
|
|
assert(self);
|
|
|
|
monMode = self->modeNode->value.v.text[0];
|
|
//XXX nopause SetModeNode(self, EVError);
|
|
switch(self->errNode->value.v.text[0]) {
|
|
case 'l':
|
|
case 'L': /* Lazy */
|
|
iStatus = 1;
|
|
break;
|
|
case 'p':
|
|
case 'P': /* Pause */
|
|
pExe = GetExecutor();
|
|
if(IsCounting(pExe)) {
|
|
if (!self->iPaused) {
|
|
SCWrite(GetExeOwner(pExe),"Pausing till OK",eError);
|
|
}
|
|
PauseExecution(pExe);
|
|
self->iPaused = 1;
|
|
//XXX nopause SetModeNode(self, EVPaused);
|
|
iStatus = 1;
|
|
}
|
|
break;
|
|
case 'i':
|
|
case 'I': /* Interrupt */
|
|
/* TODO How is this supposed to work?
|
|
SetInterrupt((int)ObVal(self->pParam,INTERRUPT));*/
|
|
iStatus = 1;
|
|
break;
|
|
case 's':
|
|
case 'S': /* Safe, run to a safe place, put him into idle afterwards */
|
|
/*TODO Drive environment to a safevalue */
|
|
break;
|
|
default:
|
|
return 0;
|
|
|
|
}
|
|
/* NOTE: emon ignores the return value */
|
|
return iStatus;
|
|
}
|
|
/*----------------------------------------------------------------
|
|
returns NULL on failure, a new datastructure else
|
|
------------------------------------------------------------------*/
|
|
static pSctEmon SCTEMONMakeObject(){
|
|
pSctEmon self = NULL;
|
|
|
|
self = calloc(sizeof(SctEmon),1);
|
|
if(self == NULL){
|
|
return NULL;
|
|
}
|
|
self->pDes = CreateDescriptor("SctEmonAdapter");
|
|
if(self->pDes == NULL) {
|
|
free(self);
|
|
return NULL;
|
|
}
|
|
self->pEVI = CreateEVInterface();
|
|
if(self->pEVI == NULL) {
|
|
DeleteDescriptor(self->pDes);
|
|
free(self);
|
|
return NULL;
|
|
}
|
|
self->pDrivInt = CreateDrivableInterface();
|
|
if(self->pDrivInt == NULL) {
|
|
DeleteDescriptor(self->pDes);
|
|
free(self->pEVI);
|
|
free(self);
|
|
return NULL;
|
|
}
|
|
|
|
self->pDes->GetInterface = SCTEmonGetInterface;
|
|
self->pEVI->GetMode = SCTEmonGetMode;
|
|
self->pEVI->IsInTolerance = SCTEmonIsInTolerance;
|
|
self->pEVI->HandleError = SCTEmonHandleError;
|
|
|
|
self->pDrivInt->Halt = NULL;
|
|
self->pDrivInt->CheckLimits = NULL;
|
|
self->pDrivInt->SetValue = NULL;
|
|
self->pDrivInt->CheckStatus = NULL;
|
|
self->pDrivInt->GetValue = SCTEmonGetValue;
|
|
return self;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
static hdbCallbackReturn SctDummyCallback(Hdb *node, void *userData,
|
|
hdbMessage *msg) {
|
|
return hdbContinue;
|
|
}
|
|
/*----------------------------------------------------------------*/
|
|
static void SctEmonDeleteNode(void *pData) {
|
|
pSctEmon self = (pSctEmon)pData;
|
|
self->modeNode = NULL;
|
|
self->tolNode = NULL;
|
|
self->errNode = NULL;
|
|
}
|
|
/*--------------------------------------------------------------
|
|
args: name, modepath, tolpath, errpath
|
|
*/
|
|
int SctMakeEmonAdapter(SConnection *pCon, SicsInterp *pSics, void *object,
|
|
int argc, char *argv[]) {
|
|
pSctEmon pNew = NULL;
|
|
hdbCallback *cb;
|
|
|
|
if(argc < 5){
|
|
SCWrite(pCon,"ERROR: not enough arguments for SctMakeEmonAdapter", eError);
|
|
return 0;
|
|
}
|
|
|
|
pNew = SCTEMONMakeObject();
|
|
if(pNew == NULL){
|
|
SCWrite(pCon,"ERROR: out of memory in SctMakeEmonAdapter",
|
|
eError);
|
|
return 0;
|
|
}
|
|
|
|
pNew->pName = strdup(argv[1]);
|
|
pNew->modeNode = FindHdbNode(NULL,argv[2], pCon);
|
|
pNew->tolNode = FindHdbNode(NULL,argv[3], pCon);
|
|
pNew->errNode = FindHdbNode(NULL,argv[4], pCon);
|
|
pNew->iPaused = 0;
|
|
|
|
/*XXX I'm guessing that SctEmonDeleteNode will be called when the
|
|
script context object is deleted. So the emon functions should check
|
|
if node == NULL before trying to do something */
|
|
cb = MakeHipadabaCallback(SctDummyCallback, pNew, SctEmonDeleteNode);
|
|
assert(cb);
|
|
AppendHipadabaCallback(pNew->modeNode, cb);
|
|
|
|
EVRegisterController(FindEMON(pSics),argv[1],pNew, pCon);
|
|
return 1;
|
|
}
|
|
/*---------------------------------------------------------------*/
|
|
|
|
void SctEmonInit(void) {
|
|
AddCmd("makesctemon", SctMakeEmonAdapter);
|
|
}
|