782 lines
22 KiB
C
782 lines
22 KiB
C
/*--------------------------------------------------------------------------
|
|
P I M O T O R
|
|
|
|
This file contains the implementation of a motor driver for the Physik
|
|
Instrument C-804 DC motor controller. Plus the implementation of an
|
|
additional wrapper function for handling addtional support commands
|
|
for this motor controller.
|
|
|
|
Mark Koenencke, September 1998
|
|
----------------------------------------------------------------------------*/
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include "fortify.h"
|
|
#include "sics.h"
|
|
#include <tcl.h>
|
|
#include "hardsup/serialsinq.h"
|
|
#include "hardsup/el734_def.h"
|
|
#include "hardsup/el734fix.h"
|
|
#include "motor.h"
|
|
#include "pimotor.h"
|
|
|
|
|
|
/*================== The Driver data structure ============================*/
|
|
typedef struct __PIMoDriv {
|
|
/* general motor driver interface
|
|
fields. REQUIRED!
|
|
*/
|
|
float fUpper; /* upper limit */
|
|
float fLower; /* lower limit */
|
|
char *name;
|
|
int (*GetPosition)(void *self,float *fPos);
|
|
int (*RunTo)(void *self, float fNewVal);
|
|
int (*GetStatus)(void *self);
|
|
void (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
|
|
int (*TryAndFixIt)(void *self,int iError, float fNew);
|
|
int (*Halt)(void *self);
|
|
int (*GetDriverPar)(void *self, char *name,
|
|
float *value);
|
|
int (*SetDriverPar)(void *self,SConnection *pCon,
|
|
char *name, float newValue);
|
|
void (*ListDriverPar)(void *self, char *motorName,
|
|
SConnection *pCon);
|
|
void (*KillPrivate)(void *self);
|
|
|
|
|
|
/* C-804 specific fields */
|
|
int iPort;
|
|
char *hostname;
|
|
int iChannel;
|
|
int iMotor;
|
|
void *pSerial;
|
|
int iLastError;
|
|
} C804Driv, *pC804Driv;
|
|
|
|
#define PITMO -845
|
|
#define PICOMERR -846
|
|
/*--------------------------------------------------------------------------*/
|
|
static int PIPosition(void *pData, float *fPos)
|
|
{
|
|
pC804Driv self = NULL;
|
|
int iRet;
|
|
char pCommand[20];
|
|
char pReply[80];
|
|
char *pPtr;
|
|
|
|
self = (pC804Driv)pData;
|
|
assert(self);
|
|
|
|
/* format command */
|
|
sprintf(pCommand,"%1.1dTP\r",self->iMotor);
|
|
|
|
/* send command */
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand, pReply, 79);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
*fPos = -99999;
|
|
return HWFault;
|
|
}
|
|
|
|
/* catch TMO or bad reply */
|
|
if(strstr(pReply,"TMO") != NULL)
|
|
{
|
|
self->iLastError = PITMO;
|
|
*fPos = -9999;
|
|
return HWFault;
|
|
}
|
|
if(strstr(pReply,"?") != NULL)
|
|
{
|
|
self->iLastError = PICOMERR;
|
|
*fPos = -9999;
|
|
return HWFault;
|
|
}
|
|
|
|
|
|
/* read value */
|
|
pPtr = pReply + 3;
|
|
*fPos = atof(pPtr);
|
|
return OKOK;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int PIRun(void *pData, float fVal)
|
|
{
|
|
pC804Driv self = NULL;
|
|
int iRet;
|
|
char pCommand[20];
|
|
char pReply[80];
|
|
int iTmo;
|
|
|
|
self = (pC804Driv)pData;
|
|
assert(self);
|
|
|
|
/* format drive command */
|
|
sprintf(pCommand,"%1.1dMA%10.10d\r",self->iMotor,(int)fVal);
|
|
|
|
iTmo = SerialGetTmo(&self->pSerial);
|
|
SerialConfig(&self->pSerial,0);
|
|
|
|
/* send command */
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand, pReply, 79);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
return HWFault;
|
|
}
|
|
|
|
/* reset tmo */
|
|
SerialConfig(&self->pSerial,iTmo);
|
|
|
|
/* wait a little to allow to gather some speed before checking on it */
|
|
SicsWait(2);
|
|
return OKOK;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static void PIError(void *pData, int *iCode, char *pError, int iErrLen)
|
|
{
|
|
pC804Driv self = NULL;
|
|
|
|
self = (pC804Driv)pData;
|
|
assert(self);
|
|
|
|
*iCode = self->iLastError;
|
|
|
|
if(*iCode == PITMO)
|
|
{
|
|
strncpy(pError,"Timeout at serial line", iErrLen);
|
|
return;
|
|
}
|
|
if(*iCode == PICOMERR)
|
|
{
|
|
strncpy(pError,"C-804 Command Error",iErrLen);
|
|
return;
|
|
}
|
|
|
|
SerialError(self->iLastError,pError,iErrLen);
|
|
return;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int PIFixError(void *pData, int iError, float fNew)
|
|
{
|
|
pC804Driv self = NULL;
|
|
int iRet;
|
|
|
|
self = (pC804Driv)pData;
|
|
assert(self);
|
|
|
|
switch(iError)
|
|
{
|
|
/* network errors */
|
|
case NOCONNECTION:
|
|
case EL734__BAD_FLUSH:
|
|
case EL734__BAD_RECV:
|
|
case EL734__BAD_RECV_NET:
|
|
case EL734__BAD_RECV_UNKN:
|
|
case EL734__BAD_RECVLEN:
|
|
case EL734__BAD_RECV1:
|
|
case EL734__BAD_RECV1_PIPE:
|
|
case EL734__BAD_RNG:
|
|
case EL734__BAD_SEND:
|
|
case EL734__BAD_SEND_PIPE:
|
|
case EL734__BAD_SEND_NET:
|
|
case EL734__BAD_SEND_UNKN:
|
|
case EL734__BAD_SENDLEN:
|
|
SerialClose(&self->pSerial);
|
|
iRet = SerialOpen(&self->pSerial,self->hostname,
|
|
self->iPort, self->iChannel);
|
|
if(iRet != 1)
|
|
{
|
|
return MOTREDO;
|
|
}
|
|
else
|
|
{
|
|
return MOTFAIL;
|
|
}
|
|
break;
|
|
/* handable protocoll errors */
|
|
case EL734__BAD_TMO:
|
|
case PITMO:
|
|
case TIMEOUT:
|
|
return MOTREDO;
|
|
break;
|
|
case PICOMERR:
|
|
return MOTFAIL;
|
|
break;
|
|
default:
|
|
return MOTFAIL;
|
|
break;
|
|
}
|
|
return MOTFAIL;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static int PIHalt(void *pData)
|
|
{
|
|
pC804Driv self = NULL;
|
|
char pCommand[20], pReply[80];
|
|
int iTmo;
|
|
|
|
self = (pC804Driv)pData;
|
|
assert(self);
|
|
|
|
sprintf(pCommand,"%1.1dAB\r",self->iMotor);
|
|
|
|
iTmo = SerialGetTmo(&self->pSerial);
|
|
SerialConfig(&self->pSerial,0);
|
|
SerialWriteRead(&self->pSerial,pCommand, pReply, 79);
|
|
SerialConfig(&self->pSerial,iTmo);
|
|
|
|
return 1;
|
|
}
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
/*-------------------------------------------------------------------------*/
|
|
static int PIStatus(void *pData)
|
|
{
|
|
pC804Driv self = NULL;
|
|
char pCommand[20], pReply[80], *pPtr;
|
|
int iRet;
|
|
float fSpeed;
|
|
|
|
self = (pC804Driv)pData;
|
|
assert(self);
|
|
|
|
/* read actual velocity: should be 0 when done */
|
|
sprintf(pCommand,"%1.1dTV\r",self->iMotor);
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand,pReply,79);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
return HWFault;
|
|
}
|
|
/* check reply */
|
|
if(strstr(pReply,"TMO") != NULL)
|
|
{
|
|
self->iLastError = PITMO;
|
|
return HWFault;
|
|
}
|
|
if(strstr(pReply,"?") != NULL)
|
|
{
|
|
self->iLastError = PICOMERR;
|
|
return HWFault;
|
|
}
|
|
|
|
pPtr = pReply+3;
|
|
fSpeed = atof(pPtr);
|
|
if(ABS(fSpeed) > 0)
|
|
{
|
|
return HWBusy;
|
|
}
|
|
return HWIdle;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static void KillC804(pC804Driv self)
|
|
{
|
|
if(!self)
|
|
return;
|
|
|
|
if(self->name)
|
|
{
|
|
free(self->name);
|
|
}
|
|
if(self->hostname)
|
|
{
|
|
free(self->hostname);
|
|
}
|
|
if(self->pSerial)
|
|
{
|
|
SerialClose(&self->pSerial);
|
|
}
|
|
free(self);
|
|
}
|
|
/*--------------------------------------------------------------------------
|
|
* The data necessary for initialising the C804 motor is contained in a
|
|
* Tcl-Array given as pArray parameter. In case of an error the error is
|
|
* returned in the Tcl-interpreter.
|
|
*/
|
|
static pC804Driv MakeC804Driver(Tcl_Interp *pTcl, char *pArray)
|
|
{
|
|
pC804Driv pNew = NULL;
|
|
int iRet, iVal, iTmo;
|
|
double dVal;
|
|
char *pPar = NULL;
|
|
char pCommand[20], pReply[40];
|
|
|
|
/* allocate space: the final frontier */
|
|
pNew = (pC804Driv)malloc(sizeof(C804Driv));
|
|
if(!pNew)
|
|
{
|
|
Tcl_SetResult(pTcl,"Out of memory for C804 Driver",NULL);
|
|
return NULL;
|
|
}
|
|
memset(pNew,0,sizeof(C804Driv));
|
|
|
|
/* connection parameters */
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"Computer",TCL_LEAVE_ERR_MSG);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find serial port server host name",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->hostname = strdup(pPar);
|
|
|
|
pPar = NULL;
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"port",TCL_LEAVE_ERR_MSG);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find serial port server port adress",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetInt(pTcl,pPar,&iVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert port adress to integer",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->iPort = iVal;
|
|
|
|
pPar = NULL;
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"channel",TCL_LEAVE_ERR_MSG);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find serial port server channel adress",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetInt(pTcl,pPar,&iVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert channel number to integer",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->iChannel = iVal;
|
|
pPar = NULL;
|
|
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"motor",TCL_LEAVE_ERR_MSG);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find motor number",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetInt(pTcl,pPar,&iVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert motor number to integer",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->iMotor = iVal;
|
|
|
|
/* limits */
|
|
pPar = NULL;
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"lowerlimit",TCL_LEAVE_ERR_MSG);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find lower motor limit",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetDouble(pTcl,pPar,&dVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert lower limit to double",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->fLower = dVal;
|
|
|
|
pPar = NULL;
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"upperlimit",TCL_LEAVE_ERR_MSG);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find upper motor limit",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetDouble(pTcl,pPar,&dVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert motor number to integer",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->fUpper = dVal;
|
|
|
|
|
|
/* open the serialport connection */
|
|
iRet = SerialOpen(&pNew->pSerial,pNew->hostname, pNew->iPort,
|
|
pNew->iChannel);
|
|
if(iRet != 1)
|
|
{
|
|
Tcl_SetResult(pTcl,
|
|
"Failed to open connection to serial port server",NULL);
|
|
KillC804(pNew);
|
|
return NULL;
|
|
}
|
|
SerialConfig(&pNew->pSerial,5000);
|
|
|
|
/* switch on, just to make sure */
|
|
iTmo = SerialGetTmo(&pNew->pSerial);
|
|
SerialConfig(&pNew->pSerial,0);
|
|
sprintf(pCommand,"%1.1dMN\r",pNew->iMotor);
|
|
SerialWriteRead(&pNew->pSerial,pCommand,pReply,39);
|
|
SerialConfig(&pNew->pSerial,iTmo+5000);
|
|
|
|
/* configure the connection */
|
|
SerialATerm(&pNew->pSerial,"1\x03");
|
|
SerialSendTerm(&pNew->pSerial,"\r");
|
|
|
|
/* configure the function pointers */
|
|
pNew->GetPosition = PIPosition;
|
|
pNew->RunTo = PIRun;
|
|
pNew->GetStatus = PIStatus;
|
|
pNew->GetError = PIError;
|
|
pNew->TryAndFixIt = PIFixError;
|
|
pNew->Halt = PIHalt;
|
|
|
|
/* success */
|
|
return pNew;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
int PISetSpeed(pMotor pMot,SConnection *pCon, float fNew)
|
|
{
|
|
int iVal, iRet, iTmo;
|
|
char pCommand[20], pReply[80], pError[132];
|
|
pC804Driv self = NULL;
|
|
|
|
assert(pMot);
|
|
self = (pC804Driv)pMot->pDriver;
|
|
assert(self);
|
|
|
|
iVal = (int)fNew;
|
|
sprintf(pCommand,"%1.1dSV%7.7d\r",self->iMotor,iVal);
|
|
iTmo = SerialGetTmo(&self->pSerial);
|
|
SerialConfig(&self->pSerial,0);
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand,pReply,79);
|
|
SerialConfig(&self->pSerial,iTmo);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
PIError(self,&iRet,pReply,79);
|
|
sprintf(pError,"ERROR: %s",pReply);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
int PIHome(pMotor pMot,SConnection *pCon)
|
|
{
|
|
int iVal, iRet, iTmo;
|
|
char pCommand[20], pReply[80], pError[132];
|
|
pC804Driv self = NULL;
|
|
|
|
assert(pMot);
|
|
self = (pC804Driv)pMot->pDriver;
|
|
assert(self);
|
|
|
|
sprintf(pCommand,"%1.1dDH\r",self->iMotor);
|
|
iTmo = SerialGetTmo(&self->pSerial);
|
|
SerialConfig(&self->pSerial,0);
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand,pReply,79);
|
|
SerialConfig(&self->pSerial,iTmo);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
PIError(self,&iRet,pReply,79);
|
|
sprintf(pError,"ERROR: %s",pReply);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
int PIOn(pMotor pMot,SConnection *pCon)
|
|
{
|
|
int iVal, iRet, iTmo;
|
|
char pCommand[20], pReply[80], pError[132];
|
|
pC804Driv self = NULL;
|
|
|
|
assert(pMot);
|
|
self = (pC804Driv)pMot->pDriver;
|
|
assert(self);
|
|
|
|
sprintf(pCommand,"%1.1dMN\r",self->iMotor);
|
|
iTmo = SerialGetTmo(&self->pSerial);
|
|
SerialConfig(&self->pSerial,0);
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand,pReply,79);
|
|
SerialConfig(&self->pSerial,iTmo);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
PIError(self,&iRet,pReply,79);
|
|
sprintf(pError,"ERROR: %s",pReply);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
int PIGetSpeed(pMotor pMot,SConnection *pCon, float *fNew)
|
|
{
|
|
int iVal, iRet;
|
|
char pCommand[20], pReply[80], pError[132], *pPtr;
|
|
pC804Driv self = NULL;
|
|
|
|
assert(pMot);
|
|
self = (pC804Driv)pMot->pDriver;
|
|
assert(self);
|
|
|
|
sprintf(pCommand,"%1.1dTY\r",self->iMotor);
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand,pReply,79);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
PIError(self,&iRet,pReply,79);
|
|
sprintf(pError,"ERROR: %s",pReply);
|
|
SCWrite(pCon,pError,eError);
|
|
return 0;
|
|
}
|
|
pPtr = pReply+3;
|
|
*fNew = atof(pPtr);
|
|
return 1;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
int PIMotorWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pMotor pMot = NULL;
|
|
double dVal;
|
|
float fVal;
|
|
char pBueffel[256];
|
|
int iRet;
|
|
|
|
pMot = (pMotor)pData;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
assert(pMot);
|
|
|
|
if(argc >= 2)
|
|
{
|
|
strtolower(argv[1]);
|
|
if(strcmp(argv[1],"speed") == 0)
|
|
{
|
|
if(argc >= 3)
|
|
{
|
|
iRet = Tcl_GetDouble(pSics->pTcl,argv[2],&dVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
SCWrite(pCon,"ERROR: speed parameter not recognised as number",
|
|
eError);
|
|
return 0;
|
|
}
|
|
if(!SCMatchRights(pCon,usMugger))
|
|
{
|
|
SCWrite(pCon,"ERROR: Insufficient privilege to change speed",
|
|
eError);
|
|
return 0;
|
|
}
|
|
iRet = PISetSpeed(pMot,pCon,(float)dVal);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
iRet = PIGetSpeed(pMot,pCon,&fVal);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
sprintf(pBueffel,"%s.speed = %f",argv[0],fVal);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
return 1;
|
|
}
|
|
} /* end speed */
|
|
if(strcmp(argv[1],"home") == 0)
|
|
{
|
|
if(!SCMatchRights(pCon,usMugger))
|
|
{
|
|
SCWrite(pCon,"ERROR: Insufficient privilege to change hardware 0",
|
|
eError);
|
|
return 0;
|
|
}
|
|
iRet = PIHome(pMot,pCon);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
if(strcmp(argv[1],"on") == 0)
|
|
{
|
|
if(!SCMatchRights(pCon,usUser))
|
|
{
|
|
SCWrite(pCon,"ERROR: Insufficient privilege to switch motor on",
|
|
eError);
|
|
return 0;
|
|
}
|
|
iRet = PIOn(pMot,pCon);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
SCSendOK(pCon);
|
|
return 1;
|
|
}
|
|
if(strcmp(argv[1],"list") == 0)
|
|
{
|
|
iRet = PIGetSpeed(pMot,pCon,&fVal);
|
|
if(!iRet)
|
|
{
|
|
return 0;
|
|
}
|
|
sprintf(pBueffel,"%s.speed = %f",argv[0],fVal);
|
|
SCWrite(pCon,pBueffel,eValue);
|
|
return MotorAction(pCon,pSics,pData,argc,argv);
|
|
}
|
|
|
|
}/* end arguments */
|
|
|
|
/* forward everything else to main motor handler */
|
|
return MotorAction(pCon,pSics,pData,argc,argv);
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static void PIMotorKill(void *self)
|
|
{
|
|
pMotor pM;
|
|
assert(self);
|
|
|
|
pM = (pMotor)self;
|
|
|
|
if(pM->name)
|
|
free(pM->name);
|
|
|
|
if(pM->pDrivInt)
|
|
{
|
|
free(pM->pDrivInt);
|
|
}
|
|
|
|
if(pM->pCall)
|
|
{
|
|
DeleteCallBackInterface(pM->pCall);
|
|
}
|
|
|
|
/* kill driver */
|
|
if(pM->drivername)
|
|
{
|
|
if(strcmp(pM->drivername,"c804") == 0)
|
|
{
|
|
if(pM->pDriver)
|
|
{
|
|
KillC804((pC804Driv)pM->pDriver);
|
|
}
|
|
}
|
|
else if(strcmp(pM->drivername,"SIM") == 0)
|
|
{
|
|
if(pM->pDriver)
|
|
{
|
|
KillSIM((void *)pM->pDriver);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(pM->drivername)
|
|
{
|
|
free(pM->drivername);
|
|
}
|
|
|
|
/* kill Descriptor */
|
|
DeleteDescriptor(pM->pDescriptor);
|
|
|
|
free(pM);
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
int PIMotorFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
|
|
int argc, char *argv[])
|
|
{
|
|
pMotor pNew = NULL;
|
|
MotorDriver *pDriver = NULL;
|
|
char pBueffel[512];
|
|
int iD, iRet;
|
|
Tcl_Interp *ppTcl;
|
|
|
|
assert(pCon);
|
|
assert(pSics);
|
|
|
|
/* a first check */
|
|
if(argc < 3)
|
|
{
|
|
SCWrite(pCon,"Insufficient arguments for motor creation",eError);
|
|
return 0;
|
|
}
|
|
|
|
/* create the driver */
|
|
strtolower(argv[2]);
|
|
strtolower(argv[1]);
|
|
if(strcmp(argv[2],"c804") == 0)
|
|
{
|
|
pDriver = (MotorDriver *)MakeC804Driver(pSics->pTcl,argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
ppTcl = (Tcl_Interp *)pSics->pTcl;
|
|
SCWrite(pCon,ppTcl->result,eError);
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("c804",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else if (strcmp(argv[2],"sim") == 0)
|
|
{
|
|
iD = argc - 3;
|
|
pDriver = CreateSIM(pCon,iD,&argv[3]);
|
|
if(!pDriver)
|
|
{
|
|
return 0;
|
|
}
|
|
/* create the motor */
|
|
pNew = MotorInit("SIM",argv[1],pDriver);
|
|
if(!pNew)
|
|
{
|
|
sprintf(pBueffel,"Failure to create motor %s",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
sprintf(pBueffel,"Motor Type %s cot recognized for motor %s",
|
|
argv[2],argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
|
|
/* create the interpreter command */
|
|
iRet = AddCommand(pSics,argv[1],PIMotorWrapper,PIMotorKill,pNew);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pBueffel,"ERROR: duplicate command %s not created",argv[1]);
|
|
SCWrite(pCon,pBueffel,eError);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
|