linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
16ad9ed8e29814a5a93bc612589ca7cac64c9326
sics/site_ansto/ansto_sctdriveadapter.c

367 lines
12 KiB
C
Raw Blame History

/**
* This is an adapter to a node under the control of the new
* scriptcontext generic device model. This is a wrapper around
* such a node which implements the drivable interface.
*
* Some cooperation from the node is required: It has to provide
* certain properties the value of which define scripts which
* have to be called at various stages. These are:
*
* checklimits, for limits checking
* checkstatus, for evaluating progress
* halt , for halting things
*
* copyright: see file COPYRIGHT
*
* Mark Koennecke, June 2008
* --------------------------------------------------------------*/
#include <sics.h>
#include <sicshipadaba.h>
#include <devser.h>
#include <tcl.h>
#include <macro.h>
#include <sicsobj.h>
#include <math.h>
#include "scriptcontext.h"
/*---------------------------------------------------------------*/
typedef struct {
pObjectDescriptor pDes;
pIDrivable pDriv;
pHdb write_node;
pHdb read_node;
SctController *c;
char* controller_name;
}SctDrive, *pSctDrive;
/*---------------------------------------------------------------*/
static void *SCTDRIVGetInterface(void *data, int iD){
pSctDrive self = NULL;
self = (pSctDrive)data;
if(self != NULL && iD == DRIVEID){
if (self->write_node == NULL) return NULL;
return self->pDriv;
} else {
return NULL;
}
return NULL;
}
/*----------------------------------------------------------------
This routine can return either OKOK or HWFault when thing
go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int SCTDRIVHalt(void *data) {
pSctDrive self = NULL;
char dummy[16];
self = (pSctDrive)data;
if (GetHdbProperty(self->write_node,"halt", dummy, sizeof dummy)) {
SctQueueNode(self->c, self->write_node, HaltPRIO, "halt", NULL);
} else if (GetHdbProperty(self->write_node, "status", dummy, sizeof dummy)) {
SetHdbProperty(self->write_node, "status", "idle");
}
return OKOK;
}
/*----------------------------------------------------------------
This routine can return either 1 or 0. 1 means the position can
be reached, 0 NOT
If 0, error shall contain up to errlen characters of information
about which limit was violated
------------------------------------------------------------------*/
static int SCTDRIVCheckLimits(void *data, float val,
char *error, int errlen){
pSctDrive self = NULL;
char script[1024];
int status;
Tcl_Interp *pTcl = NULL;
char *result;
self = (pSctDrive)data;
snprintf(script,1024,"%f", val);
SetHdbProperty(self->write_node,"target", script);
if(GetHdbProperty(self->write_node,"checklimits",script,1024)){
status = SctCallInContext(pServ->dummyCon, script,
self->write_node, self->c, &result);
if(SctVerbose(self->c)){
SCPrintf(pServ->dummyCon, eWarning, "script %s called with result %s\n ",
script, result);
}
if(status == 0){
strncpy(error,result,errlen);
return 0;
}
}
return 1;
}
/*----------------------------------------------------------------
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
------------------------------------------------------------------*/
static long SCTDRIVSetValue(void *data, SConnection *pCon, float val){
pSctDrive self = NULL;
int status;
hdbValue v;
self = (pSctDrive)data;
if (self->write_node->value.dataType == HIPINT) {
v.dataType = HIPINT;
v.v.intValue = round(val);
}
else if (self->write_node->value.dataType == HIPFLOAT) {
v.dataType = HIPFLOAT;
v.v.doubleValue = (double)val;
}
else {
SCPrintf(pCon, eError, "Node %s type (%d) is not INT nor FLOAT",
self->read_node->name, self->read_node->value.dataType);
return HWFault;
}
SetHdbProperty(self->write_node,"writestatus", "start");
status = SetHipadabaPar(self->write_node, v, pCon);
if(status == 1){
return OKOK;
} else {
return HWFault;
}
}
/*----------------------------------------------------------------
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
------------------------------------------------------------------*/
static int SCTDRIVCheckStatus(void *data, SConnection *pCon){
pSctDrive self = NULL;
char script[1024];
int status;
Tcl_Interp *pTcl = NULL;
char *result;
SConnection *con;
self = (pSctDrive)data;
/*
* check if the write command has gone through
*/
if(GetHdbProperty(self->write_node,"writestatus", script,1024)){
if(strcmp(script,"start") == 0){
return HWBusy;
}
}
/*
* run the checkstatus script
*/
if(!GetHdbProperty(self->write_node,"checkstatus",script,1024)){
if (!GetHdbProperty(self->write_node,"status",script,1024)){
SCWrite(pCon,
"ERROR: configuration problem: no checkstatus script!", eError);
return HWFault;
}
result = script;
} else {
status = SctCallInContext(pCon,script, self->write_node,
self->c, &result);
if (status == 0) {
SCPrintf(pCon,eError," script %s returned %s",
script, result);
return HWFault;
}
if(SctVerbose(self->c)){
SCPrintf(pCon,eError," script %s returned %s",
script, result);
}
}
if(strstr(result,"busy") != NULL){
return HWBusy;
} else if(strstr(result,"posfault") != NULL){
return HWPosFault;
} else if(strstr(result,"fault") != NULL){
return HWFault;
} else if(strstr(result,"idle") != NULL){
return HWIdle;
} else {
SCPrintf(pCon,eError,
"ERROR: invalid status code %s returned from checkstatus script",
result);
return HWFault;
}
return HWFault;
}
/*----------------------------------------------------------------
GetValue is supposed to read a motor position
On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float SCTDRIVGetValue(void *data, SConnection *pCon){
pSctDrive self = NULL;
float val = -99999999.99;
int status;
char error[256];
hdbValue v;
self = (pSctDrive)data;
if(GetHdbProperty(self->read_node,"geterror", error, 256)){
SCWrite(pCon,error, eError);
return val;
}
if (self->read_node->value.dataType == HIPINT)
return (float) self->read_node->value.v.intValue;
if (self->read_node->value.dataType == HIPFLOAT)
return (float)self->read_node->value.v.doubleValue;
SCPrintf(pCon, eError, "Node %s type (%d) is not INT nor FLOAT",
self->read_node->name, self->read_node->value.dataType);
return val;
}
/*----------------------------------------------------------------
returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static pSctDrive SCTDRIVMakeObject(){
pSctDrive self = NULL;
self = calloc(sizeof(SctDrive),1);
if(self == NULL){
return NULL;
}
self->pDes = CreateDescriptor("SctDriveAdapter");
self->pDriv = CreateDrivableInterface();
if(self->pDes == NULL || self->pDriv == NULL){
free(self);
return NULL;
}
self->pDes->GetInterface = SCTDRIVGetInterface;
self->pDriv->Halt = SCTDRIVHalt;
self->pDriv->CheckLimits = SCTDRIVCheckLimits;
self->pDriv->SetValue = SCTDRIVSetValue;
self->pDriv->CheckStatus = SCTDRIVCheckStatus;
self->pDriv->GetValue = SCTDRIVGetValue;
return self;
}
/*-----------------------------------------------------------------*/
static int SctDriveCommand(SConnection *pCon, SicsInterp *sics, void *object,
int argc, char *argv[]) {
pSctDrive self = (pSctDrive)object;
float val;
assert(self != NULL);
if (self->write_node == NULL || self->read_node == NULL) {
SCWrite(pCon, "ERROR: defunct object", eError);
return 0;
}
if (argc > 1) {
SCPrintf(pCon, eLog, "SctDriveCommand for %s: %s", argv[0], argv[1]);
if (strcasecmp(argv[1], "list") == 0) {
SCPrintf(pCon, eValue, "%s.write_node = %s", argv[0], GetHipadabaPath(self->write_node));
SCPrintf(pCon, eValue, "%s.read_node = %s", argv[0], GetHipadabaPath(self->read_node));
SCPrintf(pCon, eValue, "%s.controller = %s", argv[0], self->controller_name);
return 1;
}
return 0;
}
/*
* only action: print value
*/
val = self->pDriv->GetValue(self,pCon);
SCPrintf(pCon,eValue,"%s = %f", argv[0], val);
return 1;
}
/*----------------------------------------------------------------*/
static void SctDriveKill(void *data){
pSctDrive self = (pSctDrive)data;
if(self == NULL){
return;
}
if(self->pDriv != NULL){
free(self->pDriv);
}
if(self->controller_name != NULL){
free(self->controller_name);
}
if(self->pDes != NULL){
DeleteDescriptor(self->pDes);
}
free(self);
}
/*----------------------------------------------------------------*/
static hdbCallbackReturn SctDummyCallback(Hdb *node, void *userData,
hdbMessage *msg) {
return hdbContinue;
}
/*----------------------------------------------------------------*/
static void SctDriveDeleteNode(void *data) {
pSctDrive self = (pSctDrive)data;
self->write_node = NULL;
}
/*---------------------------------------------------------------*/
int ANSTO_SctMakeDriveAdapter(SConnection *pCon, SicsInterp *pSics, void *object,
int argc, char *argv[]) {
pSctDrive pNew = NULL;
pSICSOBJ obj = NULL;
hdbCallback *cb;
if(argc < 5){
SCWrite(pCon,"ERROR: not enough arguments for ANSTO_SctMakeDriveAdapter", eError);
return 0;
}
pNew = SCTDRIVMakeObject();
if(pNew == NULL){
SCWrite(pCon,"ERROR: out of memory in ANSTO_SctMakeDriveAdapter",
eError);
return 0;
}
pNew->write_node = FindHdbNode(NULL, argv[2], pCon);
if (pNew->write_node == NULL)
SCPrintf(pCon, eError, "ERROR: write-node %s not found", argv[2]);
else if (pNew->write_node->value.dataType != HIPFLOAT && pNew->write_node->value.dataType != HIPINT) {
SCPrintf(pCon, eError, "ERROR: write-node %s not INT or FLOAT", argv[2]);
pNew->write_node = NULL;
}
pNew->read_node = FindHdbNode(NULL, argv[3], pCon);
if (pNew->read_node == NULL)
SCPrintf(pCon, eError, "ERROR: read-node %s not found", argv[3]);
else if (pNew->read_node->value.dataType != HIPFLOAT && pNew->read_node->value.dataType != HIPINT) {
SCPrintf(pCon, eError, "ERROR: read-node %s not INT or FLOAT", argv[3]);
pNew->write_node = NULL;
}
obj = FindCommandData(pSics,argv[4], "SctController");
if (obj == NULL)
SCPrintf(pCon, eError, "ERROR: controller %s not found", argv[4]);
if (pNew->write_node == NULL || pNew->read_node == NULL || obj == NULL) {
SctDriveKill(pNew);
return 0;
}
pNew->c =(SctController *)obj->pPrivate;
if (strcasecmp(argv[0],"dynsctdrive") == 0) {
/* make object dynamic by defining a descriptor command */
SetDescriptorKey(pNew->pDes, "creationCommand", "0");
}
AddCommand(pSics, argv[1], SctDriveCommand, SctDriveKill, pNew);
SetHdbProperty(pNew->write_node,"sicsdev",argv[1]);
pNew->controller_name = strdup(argv[4]);
cb = MakeHipadabaCallback(SctDummyCallback, pNew, SctDriveDeleteNode);
assert(cb);
AppendHipadabaCallback(pNew->write_node, cb);
return 1;
}
/*---------------------------------------------------------------*/
void ANSTO_SctDriveInit(void) {
AddCmd("ansto_makesctdrive", ANSTO_SctMakeDriveAdapter);
}
Reference in New Issue View Git Blame Copy Permalink