move tclmot to site site_ansto

r3804 | jgn | 2012-11-14 11:33:50 +1100 (Wed, 14 Nov 2012) | 1 line

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 <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include <time.h>
+#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]==' ' && c<strlen(pch)) c++;
+      char *newStr = (char *)calloc(strlen(pch)-c+1, sizeof(char));
+      if(c==strlen(pch)) {
+	*newStr = NULL;
+      } else {
+	int ind=0;
+	while(c<strlen(pch)) {
+	  newStr[ind] = pch[c];
+	  c++;
+	  ind++;
+	}
+      }
+      if (ps%2==0){
+        if (*newStr==NULL) {
+          free(newStr);
+          return OKOK;
+        } else {
+	  funcName = (char *)calloc(strlen(pch)-c+1, sizeof(char));
+	  strcpy(funcName, newStr);
+	  ps++;
+        }
+      } else {
+	StringDictAddPair(pDriv->mappings,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;
+  }
+
+
+
+
+
+
+
+
+
+