- Rearranged directory structure for forking out ANSTO
- Refactored site specific stuff into a site module - PSI specific stuff is now in the PSI directory. - The old version has been tagged with pre-ansto
This commit is contained in:
cvs
785
pimotor.c
Normal file
785
pimotor.c
Normal file
@ -0,0 +1,785 @@
|
||||
/*--------------------------------------------------------------------------
|
||||
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 <tcl.h>
|
||||
#include "fortify.h"
|
||||
#include "sics.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 = Tcl_GetVar2(pTcl,pArray,"Computer",TCL_GLOBAL_ONLY);
|
||||
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 = Tcl_GetVar2(pTcl,pArray,"port",TCL_GLOBAL_ONLY);
|
||||
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 = Tcl_GetVar2(pTcl,pArray,"channel",TCL_GLOBAL_ONLY);
|
||||
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 = Tcl_GetVar2(pTcl,pArray,"motor",TCL_GLOBAL_ONLY);
|
||||
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 = Tcl_GetVar2(pTcl,pArray,"lowerlimit",TCL_GLOBAL_ONLY);
|
||||
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 = Tcl_GetVar2(pTcl,pArray,"upperlimit",TCL_GLOBAL_ONLY);
|
||||
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;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
|
||||
/* 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);
|
||||
}
|
||||
/* get rid of parameter space */
|
||||
if(pM->ParArray)
|
||||
{
|
||||
ObParDelete(pM->ParArray);
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user