575 lines
15 KiB
C
575 lines
15 KiB
C
/*---------------------------------------------------------------------------
|
|
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.
|
|
|
|
Author: Jing Chen, Nov 2012
|
|
|
|
copyright: see file COPYRIGHT
|
|
--------------------------------------------------------------------------*/
|
|
|
|
#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);
|
|
snprintf(tclCommand,commandLen,"%s", tclFunc);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static int GetTclPos(void *self, float *fPos){
|
|
TCLDriv *pDriv;
|
|
int status;
|
|
const char *result = NULL;
|
|
|
|
assert(self);
|
|
pDriv = (TCLDriv *)self;
|
|
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
|
|
pDriv->errorCode = 0;
|
|
if(!buildStandardCommandPart(pDriv,"getpos",tclCommand,1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
|
|
status = Tcl_Eval(pServ->pSics->pTcl,tclCommand);
|
|
result = Tcl_GetStringResult(pServ->pSics->pTcl);
|
|
free(tclCommand);
|
|
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 num[80];
|
|
int status;
|
|
const char *result = NULL;
|
|
|
|
assert(self);
|
|
pDriv = (TCLDriv *)self;
|
|
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
|
|
pDriv->errorCode = 0;
|
|
if(!buildStandardCommandPart(pDriv,"run",tclCommand,1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
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);
|
|
free(tclCommand);
|
|
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 TclGetError(void *self, int *iCode, char *error, int iErrLen){
|
|
TCLDriv *pDriv;
|
|
int status = 1;
|
|
const char *result = NULL;
|
|
|
|
assert(self);
|
|
pDriv = (TCLDriv *)self;
|
|
|
|
if(evaluateInternalErrors(pDriv,iCode,error,iErrLen) == 1) {
|
|
return;
|
|
}
|
|
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
|
|
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) {
|
|
free(tclCommand);
|
|
return;
|
|
}
|
|
free(tclCommand);
|
|
strncpy(error,result,iErrLen);
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
static int TclTryAndFixIt(void *self, int iError, float fNew){
|
|
TCLDriv *pDriv;
|
|
char num[80];
|
|
int status;
|
|
const char *result = NULL;
|
|
|
|
assert(self);
|
|
pDriv = (TCLDriv *)self;
|
|
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
|
|
if(!buildStandardCommandPart(pDriv,"fixit",tclCommand,1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
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;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
if(status != TCL_OK){
|
|
pDriv->errorCode = TCLERROR;
|
|
strncpy(pDriv->tclError,result,1023);
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
sscanf(result,"%d",&status);
|
|
free(tclCommand);
|
|
return status;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static int TclHalt(void *self)
|
|
{
|
|
TCLDriv *pDriv;
|
|
int status;
|
|
const char *result = NULL;
|
|
|
|
assert(self);
|
|
pDriv = (TCLDriv *)self;
|
|
|
|
pDriv->errorCode = 0;
|
|
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
|
|
if(!buildStandardCommandPart(pDriv,"halt",tclCommand,1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
|
|
status = Tcl_Eval(pServ->pSics->pTcl,tclCommand);
|
|
result = Tcl_GetStringResult(pServ->pSics->pTcl);
|
|
if(result == NULL){
|
|
pDriv->errorCode = NOTCLRESULT;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
if(status != TCL_OK){
|
|
pDriv->errorCode = TCLERROR;
|
|
strncpy(pDriv->tclError,result,1023);
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
free(tclCommand);
|
|
return OKOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------*/
|
|
static int TclGetStatus(void *self)
|
|
{
|
|
TCLDriv *pDriv;
|
|
int status;
|
|
const char *result = NULL;
|
|
|
|
assert(self);
|
|
pDriv = (TCLDriv *)self;
|
|
|
|
pDriv->errorCode = 0;
|
|
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
|
|
if(!buildStandardCommandPart(pDriv,"status",tclCommand,1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
|
|
status = Tcl_Eval(pServ->pSics->pTcl,tclCommand);
|
|
result = Tcl_GetStringResult(pServ->pSics->pTcl);
|
|
if(result == NULL){
|
|
pDriv->errorCode = NOTCLRESULT;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
if(status != TCL_OK){
|
|
pDriv->errorCode = TCLERROR;
|
|
strncpy(pDriv->tclError,result,1023);
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
sscanf(result,"%d",&status);
|
|
free(tclCommand);
|
|
return status;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
static int TclSetPar(void *self, SConnection *pCon, char *name, float newValue){
|
|
|
|
int status;
|
|
const char *result = NULL;
|
|
|
|
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 {
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
if (StringDictExists(pDriv->mappings, name)) {
|
|
if (!buildStandardCommandPart(pDriv, name, tclCommand, 1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
char *tclCommandPara = (char *)calloc(1024, sizeof(char));
|
|
sprintf(tclCommandPara, "%s %f", tclCommand, newValue);
|
|
status = Tcl_Eval(pServ->pSics->pTcl,tclCommandPara);
|
|
result = Tcl_GetStringResult(pServ->pSics->pTcl);
|
|
if (result == NULL){
|
|
pDriv->errorCode = NOTCLRESULT;
|
|
free(tclCommand);
|
|
free(tclCommandPara);;
|
|
return HWFault;
|
|
}
|
|
if (status != TCL_OK){
|
|
pDriv->errorCode = TCLERROR;
|
|
strncpy(pDriv->tclError,result,1023);
|
|
free(tclCommand);
|
|
free(tclCommandPara);;
|
|
return HWFault;
|
|
}
|
|
free(tclCommand);
|
|
free(tclCommandPara);;
|
|
return OKOK;
|
|
} else {
|
|
pDriv->errorCode = PARANOTFOUND;
|
|
return 0;
|
|
}
|
|
|
|
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) {
|
|
int ret;
|
|
float value;
|
|
ret = TclGetPar(self, sVal.name, &value);
|
|
if (ret != OKOK) {
|
|
return ret;
|
|
} else {
|
|
snprintf(pBueffel,511,"%s.%s = %f",motorname,sVal.name,value);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
}
|
|
}
|
|
iRet = LLDnodePtr2Next(pDriv->mappings->iList);
|
|
}
|
|
|
|
return OKOK;
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
int TclGetPar(void *self, char *name, float *value) {
|
|
|
|
int status;
|
|
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 {
|
|
char *tclCommand = (char *)calloc(1024, sizeof(char));
|
|
if (StringDictExists(pDriv->mappings, name)) {
|
|
if(!buildStandardCommandPart(pDriv, name, tclCommand, 1023)){
|
|
pDriv->errorCode = FUNCNOTFOUND;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
status = Tcl_Eval(pServ->pSics->pTcl,tclCommand);
|
|
result = Tcl_GetStringResult(pServ->pSics->pTcl);
|
|
if(result == NULL){
|
|
pDriv->errorCode = NOTCLRESULT;
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
if(status != TCL_OK){
|
|
pDriv->errorCode = TCLERROR;
|
|
strncpy(pDriv->tclError,result,1023);
|
|
free(tclCommand);
|
|
return HWFault;
|
|
}
|
|
sscanf(result,"%f", value);
|
|
free(tclCommand);
|
|
return OKOK;
|
|
} else {
|
|
pDriv->errorCode = PARANOTFOUND;
|
|
free(tclCommand);
|
|
return 0;
|
|
}
|
|
|
|
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;
|
|
char *funcName = (char *)calloc(1024, sizeof(char));
|
|
|
|
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);
|
|
free(funcName);
|
|
return OKOK;
|
|
} else {
|
|
strcpy(funcName, newStr);
|
|
ps++;
|
|
}
|
|
} else {
|
|
StringDictAddPair(pDriv->mappings,funcName, newStr);
|
|
ps = 0;
|
|
}
|
|
free(newStr);
|
|
pch = strtok (NULL, "{}");
|
|
}
|
|
free(funcName);
|
|
}
|
|
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 = TclGetStatus;
|
|
pDriv->GetError = TclGetError;
|
|
pDriv->TryAndFixIt = TclTryAndFixIt;
|
|
pDriv->SetDriverPar = TclSetPar;
|
|
pDriv->GetDriverPar = TclGetPar;
|
|
pDriv->ListDriverPar = TclListPar;
|
|
pDriv->Halt = TclHalt;
|
|
pDriv->KillPrivate = KillTCL;
|
|
|
|
return (MotorDriver *)pDriv;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|