init

r3786 | jgn | 2012-11-09 14:12:28 +1100 (Fri, 09 Nov 2012) | 1 line

site_ansto/jgn-tclmotdriv.c

@@ -0,0 +1,511 @@
+/*---------------------------------------------------------------------------
+ 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 "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 = PARANOTFOUND;
+      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;
+  }
+
+  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 = PARANOTFOUND;  
+      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;
+  }
+
+  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;
+  }
+
+
+
+
+
+
+
+
+
+