From 74c4995b4ba6702b9fc2c8805562114e9c7c409e Mon Sep 17 00:00:00 2001 From: Jing Chen Date: Tue, 13 Nov 2012 16:40:56 +1100 Subject: [PATCH] implement tclmot r3800 | jgn | 2012-11-13 16:40:56 +1100 (Tue, 13 Nov 2012) | 1 line --- site_ansto/jgn-tclmotdriv.c | 517 ------------------------------------ 1 file changed, 517 deletions(-) delete mode 100644 site_ansto/jgn-tclmotdriv.c diff --git a/site_ansto/jgn-tclmotdriv.c b/site_ansto/jgn-tclmotdriv.c deleted file mode 100644 index 2404b960..00000000 --- a/site_ansto/jgn-tclmotdriv.c +++ /dev/null @@ -1,517 +0,0 @@ -/*--------------------------------------------------------------------------- - 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 "tclmotdriv.h" - -#define FUNCNOTFOUND -11000 -#define TCLERROR -11001 -#define NOTCLRESULT -11002 -#define PARANOTFOUND -11003 - -#define UNITS "units" -#define SPEED "speed" -#define ACCEL "accel" -#define DECEL "decel" -#define LONG_NAME "long_name" -#define HARDLOWERLIM "hardlowerlim" -#define HARDUPPERLIM "hardupperlim" -#define ERRORCODE "errorcode" - - -const char *ParaText[] = {"accel", - "decel", - "setpoint", - "speed", - NULL}; - -/*----------------------------------------------------------------------------*/ -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; - } - if (strcmp(name, HARDLOWERLIM) == 0) { - pDriv->fLower = newValue; - return 1; - } - if (strcmp(name, ERRORCODE) == 0) { - pDriv->errorCode = (int)newValue; - return 1; - } - if (strcmp(name, LONG_NAME) == 0) { - strncpy(pDriv->long_name, &newValue, 255); - return 1; - } - - if (strcmp(name, SPEED) == 0 || - strcmp(name, ACCEL) == 0 || - strcmp(name, DECEL) == 0) { - 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 0; -} - -static int TclListPar(void *self, char *motorname, SConnection *pCon) { - - float value; - char pBueffel[512]; - TCLDriv *pDriv = (TCLDriv *)self; - - int count = 0; - while(ParaText[count] != NULL) { - if (StringDictExists(pDriv->mappings, ParaText[count]) == 1) { - if (TclGetPar(pDriv, ParaText[count], &value) != OKOK) { - pDriv->errorCode = PARANOTFOUND; - return HWFault; - } - snprintf(pBueffel,511,"%s.%s = %f",motorname,ParaText[count],&value); - SCWrite(pCon,pBueffel,eValue); - } - count++; - } -} - - -/*-----------------------------------------------------------------------*/ -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; - } - if (strcmp(name, HARDUPPERLIM) == 0) { - *value = (float)pDriv->fUpper; - return OKOK; - } - if (strcmp(name, HARDLOWERLIM) == 0) { - *value = (float)pDriv->fLower; - return OKOK; - } - - if (strcmp(name, SPEED) == 0 || - strcmp(name, ACCEL) == 0 || - strcmp(name, DECEL) == 0) { - 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 0; -} -/*---------------------------------------------------------------------------*/ - void KillTCL(void *pData) - { - TCLDriv *pDriv = (TCLDriv *) pData; - - if(pDriv != NULL){ - DeleteStringDict(pDriv->mappings); - } - - return; - } -/*-------------------------------------------------------------------------*/ -static int assignMappings(TCLDriv *pDriv, SConnection *pCon, char *arrayName){ - const char *funcName = NULL; - const char *paraname = NULL; - char *funcText[] = {"getpos", - "run", - "status", - "halt", - "geterror", - "fixit", - NULL}; - - char error[256]; - - int count = 0; - while(funcText[count] != NULL) { - funcName = Tcl_GetVar2(pServ->pSics->pTcl,arrayName,funcText[count],TCL_GLOBAL_ONLY); - if(funcName == NULL) { - snprintf(error,255,"ERROR: entry for %s not found in tcl-array %s", - funcText[count], arrayName); - SCWrite(pCon,error,eError); - return 0; - } - StringDictAddPair(pDriv->mappings,funcText[count],(char *)funcName); - count++; - } - count = 0; - while(ParaText[count] != NULL) { - paraname = Tcl_GetVar2(pServ->pSics->pTcl,arrayName,ParaText[count],TCL_GLOBAL_ONLY); - if(paraname != NULL) { - StringDictAddPair(pDriv->mappings,ParaText[count],(char *)paraname); - } - count++; - } - 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; - } - - - - - - - - - -