diff --git a/site_ansto/ansto_tclmotdriv.c b/site_ansto/ansto_tclmotdriv.c new file mode 100644 index 00000000..8a09e198 --- /dev/null +++ b/site_ansto/ansto_tclmotdriv.c @@ -0,0 +1,523 @@ +/*--------------------------------------------------------------------------- + This is a motor driver which is implemented in Tcl. This means + this code is only a wrapper which calls Tcl functions to do the + actual work. + + The Tcl functions to implement the interface are called with the name + of the motor as first parameter followed by any additional parameters + such as the position to run to for run. Functions have to return the proper + SICS return codes for a motor driver as integer numbers. + + The Tcl function list is initialized from a Tcl-array which holds function + names for the entries: + - getpos + - run + - status + - geterror + - fixit + This Tcl-array is passed as parameter on creating the motor. In order to + facilitate error handling, a motor parameter errorcode is available to + store errors between invocations. + + copyright: see file COPYRIGHT + + Mark Koennecke, December 2005 + --------------------------------------------------------------------------*/ +#include +#include +#include +#include +#include "sics.h" +#include "modriv.h" +#include "lld.h" +#include "stringdict.h" +#include "tclmotdriv.h" + +#define FUNCNOTFOUND -11000 +#define TCLERROR -11001 +#define NOTCLRESULT -11002 +#define PARANOTFOUND -11003 + +#define HARDUPPERLIM "hardupperlim" +#define HARDLOWERLIM "hardlowerlim" +#define LONG_NAME "long_name" +#define ERRORCODE "errorcode" + + +/*----------------------------------------------------------------------------*/ +static int buildStandardCommandPart(TCLDriv *pDriv, char *command, + char *tclCommand, int commandLen){ + char tclFunc[132]; + int status; + + status = StringDictGet(pDriv->mappings,command,tclFunc,131); + if(status != 1) { + return 0; + } + snprintf(tclCommand,commandLen,"%s %s ", tclFunc, pDriv->motName); + return 1; +} +/*----------------------------------------------------------------------------*/ + static int GetTclPos(void *self, float *fPos){ + TCLDriv *pDriv; + char tclCommand[1024]; + int status; + const char *result = NULL; + + assert(self); + pDriv = (TCLDriv *)self; + + pDriv->errorCode = 0; + if(!buildStandardCommandPart(pDriv,"getpos",tclCommand,1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL){ + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if(status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + sscanf(result,"%f",fPos); + return OKOK; + } +/*----------------------------------------------------------------------------*/ + static int TclRun(void *self, float fVal) { + TCLDriv *pDriv; + char tclCommand[1024]; + char num[80]; + int status; + const char *result = NULL; + + assert(self); + pDriv = (TCLDriv *)self; + + pDriv->errorCode = 0; + if(!buildStandardCommandPart(pDriv,"run",tclCommand,1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + snprintf(num,79,"%f",fVal); + strncat(tclCommand,num,1023-strlen(tclCommand)); + + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL) { + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if(status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + sscanf(result,"%d",&status); + return status; + } +/*-------------------------------------------------------------------------*/ +static int evaluateInternalErrors(TCLDriv *pDriv, int *iCode, + char *error, int iErrLen){ + switch(pDriv->errorCode){ + case FUNCNOTFOUND: + strncpy(error,"Config Error: Tcl function for driver not found",iErrLen); + *iCode = pDriv->errorCode; + return 1; + break; + case TCLERROR: + strncpy(error,pDriv->tclError,iErrLen); + *iCode = pDriv->errorCode; + return 1; + break; + case NOTCLRESULT: + strncpy(error,"Tcl function did not return result",iErrLen); + *iCode = pDriv->errorCode; + return 1; + break; + default: + return 0; + break; + } + return 0; +} +/*--------------------------------------------------------------------------*/ + static void TclError(void *self, int *iCode, char *error, int iErrLen){ + TCLDriv *pDriv; + char tclCommand[1024]; + int status = 1; + const char *result = NULL; + + assert(self); + pDriv = (TCLDriv *)self; + + if(evaluateInternalErrors(pDriv,iCode,error,iErrLen) == 1) { + return; + } + + if(!buildStandardCommandPart(pDriv,"geterror",tclCommand,1023)){ + pDriv->errorCode = FUNCNOTFOUND; + status = 0; + } + + if(status != 0){ + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL) { + pDriv->errorCode = NOTCLRESULT; + } + if(status != TCL_OK && result != NULL){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + } + } + + if(evaluateInternalErrors(pDriv,iCode,error,iErrLen) == 1) { + return; + } + strncpy(error,result,iErrLen); + } +/*---------------------------------------------------------------------------*/ + static int TclFix(void *self, int iError, float fNew){ + TCLDriv *pDriv; + char tclCommand[1024]; + char num[80]; + int status; + const char *result = NULL; + + assert(self); + pDriv = (TCLDriv *)self; + + if(!buildStandardCommandPart(pDriv,"fixit",tclCommand,1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + snprintf(num,79,"%d %f",pDriv->errorCode, fNew); + strncat(tclCommand,num,1023-strlen(tclCommand)); + + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL) { + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if(status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + sscanf(result,"%d",&status); + return status; + } +/*--------------------------------------------------------------------------*/ + static int TclHalt(void *self) + { + TCLDriv *pDriv; + char tclCommand[1024]; + int status; + const char *result = NULL; + + assert(self); + pDriv = (TCLDriv *)self; + + pDriv->errorCode = 0; + if(!buildStandardCommandPart(pDriv,"halt",tclCommand,1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL){ + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if(status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + return OKOK; + } +/*--------------------------------------------------------------------------*/ + static int TclStat(void *self) + { + TCLDriv *pDriv; + char tclCommand[1024]; + int status; + const char *result = NULL; + + assert(self); + pDriv = (TCLDriv *)self; + + pDriv->errorCode = 0; + if(!buildStandardCommandPart(pDriv,"status",tclCommand,1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL){ + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if(status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + sscanf(result,"%d",&status); + return status; + } +/*-----------------------------------------------------------------------*/ +static int TclSetPar(void *self, SConnection *pCon, char *name, float newValue){ + + int status; + const char *result = NULL; + + char tclCommand[1024]; + char tclCommandPara[1024]; + + TCLDriv *pDriv = (TCLDriv *) self; + + assert(self); + assert(pCon); + + if (strcmp(name, HARDUPPERLIM) == 0) { + pDriv->fUpper = newValue; + return 1; + } else if (strcmp(name, HARDLOWERLIM) == 0) { + pDriv->fLower = newValue; + return 1; + } else if (strcmp(name, ERRORCODE) == 0) { + pDriv->errorCode = (int)newValue; + return 1; + } else if (strcmp(name, LONG_NAME) == 0) { + strncpy(pDriv->long_name, &newValue, 255); + return 1; + } else { + if (StringDictExists(pDriv->mappings, name)) { + if (!buildStandardCommandPart(pDriv, name, tclCommand, 1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + sscanf(tclCommandPara, "%s %f", tclCommand, &newValue); + status = Tcl_Eval(pServ->pSics->pTcl,tclCommandPara); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if (result == NULL){ + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if (status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + return OKOK; + } else { + pDriv->errorCode = PARANOTFOUND; + return HWFault; + } + + return OKOK; + } +} + +static int TclListPar(void *self, char *motorname, SConnection *pCon) { + + SDE sVal; + int iRet; + + char pBueffel[512]; + + char *funcText[] = {"getpos", + "run", + "status", + "halt", + "geterror", + "fixit", + NULL}; + + TCLDriv *pDriv = (TCLDriv *)self; + + iRet = LLDnodePtr2First(pDriv->mappings->iList); + while(iRet != 0){ + LLDnodeDataTo(pDriv->mappings->iList, &sVal); + int count=0, flag=0; + while(funcText[count]!=NULL && flag==0) { + if(strcmp(sVal.name,funcText[count]) == 0) + flag = 1; + count++; + } + if(flag==0) { + snprintf(pBueffel,511,"%s.%s = %f",motorname,sVal.name,sVal.value); + SCWrite(pCon,pBueffel,eValue); + } + iRet = LLDnodePtr2Next(pDriv->mappings->iList); + } + +} + + +/*-----------------------------------------------------------------------*/ +int TclGetPar(void *self, char *name, float *value) { + + int status; + char tclCommand[1024]; + const char *result = NULL; + + TCLDriv *pDriv = (TCLDriv *) self; + + assert(self); + + if (strcmp(name, ERRORCODE) == 0) { + *value = (float)pDriv->errorCode; + return OKOK; + } else if (strcmp(name, HARDUPPERLIM) == 0) { + *value = (float)pDriv->fUpper; + return OKOK; + } else if (strcmp(name, HARDLOWERLIM) == 0) { + *value = (float)pDriv->fLower; + return OKOK; + } else { + if (StringDictExists(pDriv->mappings, name)) { + if(!buildStandardCommandPart(pDriv, name, tclCommand, 1023)){ + pDriv->errorCode = FUNCNOTFOUND; + return HWFault; + } + status = Tcl_Eval(pServ->pSics->pTcl,tclCommand); + result = Tcl_GetStringResult(pServ->pSics->pTcl); + if(result == NULL){ + pDriv->errorCode = NOTCLRESULT; + return HWFault; + } + if(status != TCL_OK){ + pDriv->errorCode = TCLERROR; + strncpy(pDriv->tclError,result,1023); + return HWFault; + } + sscanf(result,"%f", value); + return OKOK; + } else { + pDriv->errorCode = PARANOTFOUND; + return HWFault; + } + + return OKOK; + } +} +/*---------------------------------------------------------------------------*/ + void KillTCL(void *pData) + { + TCLDriv *pDriv = (TCLDriv *) pData; + + if(pDriv != NULL){ + DeleteStringDict(pDriv->mappings); + } + + return; + } +/*-------------------------------------------------------------------------*/ +static int assignMappings(TCLDriv *pDriv, SConnection *pCon, char *arrayName){ + + if (*arrayName==NULL) { + return OKOK; + } else { + unsigned char ps=0; + char *pch, *funcName; + + pch = strtok(arrayName, "{}"); + while (pch != NULL) { + int c=0; + while(pch[c]==' ' && cmappings,funcName, newStr); + ps++; + free(funcName); + } + free(newStr); + pch = strtok (NULL, "{}"); + } + } + return 1; +} +/*--------------------------------------------------------------------------*/ + MotorDriver *CreateTclMotDriv(SConnection *pCon, int argc, char *argv[]) + { + TCLDriv *pDriv = NULL; + + assert(pCon); + + if(argc < 4) { + SCWrite(pCon,"ERROR: not enough arguments to initilaize Tcl-driver",eError); + return NULL; + } + + pDriv = (TCLDriv *)malloc(sizeof(TCLDriv)); + if(!pDriv){ + SCWrite(pCon,"Error allocating memory in TclMotor",eError); + return NULL; + } + memset(pDriv,0,sizeof(TCLDriv)); + pDriv->mappings = CreateStringDict(); + if(pDriv->mappings == NULL){ + SCWrite(pCon,"Error allocating memory in TclMotor",eError); + free(pDriv); + return NULL; + } + if(assignMappings(pDriv,pCon,argv[3]) != 1){ + DeleteStringDict(pDriv->mappings); + free(pDriv); + return NULL; + } + + pDriv->name = strdup("Tcl-Driver"); + strncpy(pDriv->motName, argv[1], 131); + pDriv->GetPosition = GetTclPos; + pDriv->RunTo = TclRun; + pDriv->GetStatus = TclStat; + pDriv->GetError = TclError; + pDriv->TryAndFixIt = TclFix; + pDriv->SetDriverPar = TclSetPar; + pDriv->GetDriverPar = TclGetPar; + pDriv->ListDriverPar = TclListPar; + pDriv->Halt = TclHalt; + pDriv->KillPrivate = KillTCL; + + return (MotorDriver *)pDriv; + } + + + + + + + + + +