linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
bd533e61317fa38434fedc33f0710abd845181bc
sics/tclmotdriv.c
koennecke 45fd50265f - Added SicsList 
- Removed group and description attributes from ObjectDescriptor


SKIPPED:
	psi/dornier2.c
	psi/libpsi.a
	psi/make_gen
	psi/makefile_linux
	psi/pimotor.c
	psi/pipiezo.c
	psi/psi.c
	psi/serial.c
	psi/sinqhttp.c
	psi/sinqhttp.h
	psi/tcpdornier.c
	psi/velodornier.c
2006-01-16 08:32:07 +00:00

395 lines
10 KiB
C
Raw Blame History

/*---------------------------------------------------------------------------
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
/*----------------------------------------------------------------------------*/
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){
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;
}
return 0;
}
/*-----------------------------------------------------------------------*/
static int TclGetPar(void *self, char *name, float *value){
TCLDriv *pDriv = (TCLDriv *) self;
assert(self);
if(strcmp(name,"errorcode") == 0)
{
*value = (float)pDriv->errorCode;
return 1;
}
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;
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++;
}
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->Halt = TclHalt;
pDriv->KillPrivate = KillTCL;
return (MotorDriver *)pDriv;
}
Reference in New Issue View Git Blame Copy Permalink