390 lines
11 KiB
C
390 lines
11 KiB
C
/*--------------------------------------------------------------------------
|
|
P I M O T O R
|
|
|
|
This file contains the implementation of a motor driver for the Physik
|
|
Instrumente Piezo Controller E-255. It is handled as a motor though it
|
|
is a Piezo device. This gets a preset voltage in millivolts and that
|
|
determines it's elongation and that is it.
|
|
|
|
Mark Koennecke, February 1999
|
|
|
|
----------------------------------------------------------------------------*/
|
|
#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"
|
|
|
|
|
|
/*================== The Driver data structure ============================*/
|
|
typedef struct __PIPiezo {
|
|
/* 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);
|
|
|
|
|
|
/* PiPiezo specific fields */
|
|
int iPort;
|
|
char *hostname;
|
|
int iChannel;
|
|
int iMotor;
|
|
void *pSerial;
|
|
int iLastError;
|
|
int iLastVolt;
|
|
}PiPiezo, *pPiPiezo;
|
|
|
|
#define PITMO -845
|
|
#define PICOMERR -846
|
|
/*--------------------------------------------------------------------------*/
|
|
static int PIPosition(void *pData, float *fPos)
|
|
{
|
|
pPiPiezo self = NULL;
|
|
int iRet;
|
|
char pCommand[20];
|
|
char pReply[80];
|
|
char *pPtr;
|
|
|
|
self = (pPiPiezo)pData;
|
|
assert(self);
|
|
|
|
/* format command */
|
|
sprintf(pCommand,"%1.1dTO\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)
|
|
{
|
|
pPiPiezo self = NULL;
|
|
int iRet;
|
|
char pCommand[20];
|
|
char pReply[80];
|
|
int iTmo;
|
|
|
|
self = (pPiPiezo)pData;
|
|
assert(self);
|
|
|
|
/* format command */
|
|
sprintf(pCommand,"%1.1dSO%5.5d\r",self->iMotor,(int)fVal);
|
|
|
|
/* send command */
|
|
iRet = SerialWriteRead(&self->pSerial,pCommand, pReply, 79);
|
|
if(iRet != 1)
|
|
{
|
|
self->iLastError = iRet;
|
|
return HWFault;
|
|
}
|
|
self->iLastVolt = (int)fVal;
|
|
|
|
return OKOK;
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
static void PIError(void *pData, int *iCode, char *pError, int iErrLen)
|
|
{
|
|
pPiPiezo self = NULL;
|
|
|
|
self = (pPiPiezo)pData;
|
|
assert(self);
|
|
|
|
*iCode = self->iLastError;
|
|
|
|
if(*iCode == PITMO)
|
|
{
|
|
strncpy(pError,"Timeout at serial line", iErrLen);
|
|
return;
|
|
}
|
|
if(*iCode == PICOMERR)
|
|
{
|
|
strncpy(pError,"E-255 Command Error",iErrLen);
|
|
return;
|
|
}
|
|
|
|
/* wait a little to give this thing a reaction time, otherwise
|
|
trouble reading responses
|
|
*/
|
|
SicsWait(2);
|
|
SerialError(self->iLastError,pError,iErrLen);
|
|
return;
|
|
}
|
|
/*--------------------------------------------------------------------------*/
|
|
static int PIFixError(void *pData, int iError, float fNew)
|
|
{
|
|
pPiPiezo self = NULL;
|
|
int iRet;
|
|
|
|
self = (pPiPiezo)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)
|
|
{
|
|
|
|
/* just drive to 0 volt */
|
|
PIRun(pData,0.0);
|
|
|
|
return 1;
|
|
}
|
|
#define ABS(x) (x < 0 ? -(x) : (x))
|
|
/*-------------------------------------------------------------------------*/
|
|
static int PIStatus(void *pData)
|
|
{
|
|
pPiPiezo self = NULL;
|
|
char pCommand[20], pReply[80], *pPtr;
|
|
int iRet;
|
|
float fSpeed;
|
|
int iSpeed, iDiff;
|
|
|
|
self = (pPiPiezo)pData;
|
|
assert(self);
|
|
|
|
/* read actual velocity: should be 0 when done */
|
|
sprintf(pCommand,"%1.1dTO",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;
|
|
iSpeed = atoi(pPtr);
|
|
iDiff = iSpeed - self->iLastVolt;
|
|
if(ABS(iDiff) > 5)
|
|
{
|
|
return HWBusy;
|
|
}
|
|
return HWIdle;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
void KillPiPiezo(void *pData)
|
|
{
|
|
pPiPiezo self = (pPiPiezo)pData;
|
|
|
|
if(!self)
|
|
return;
|
|
|
|
if(self->hostname)
|
|
{
|
|
free(self->hostname);
|
|
}
|
|
if(self->pSerial)
|
|
{
|
|
SerialClose(&self->pSerial);
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------------
|
|
* The data necessary for initialising the PiPiezo motor is contained in a
|
|
* Tcl-Array given as pArray parameter. In case of an error the error is
|
|
* returned in the Tcl-interpreter.
|
|
*/
|
|
MotorDriver *MakePiPiezo(Tcl_Interp *pTcl,char *pArray)
|
|
{
|
|
pPiPiezo pNew = NULL;
|
|
int iRet, iVal, iTmo;
|
|
double dVal;
|
|
char *pPar = NULL;
|
|
char pCommand[20], pReply[40];
|
|
|
|
/* allocate space: the final frontier */
|
|
pNew = (pPiPiezo)malloc(sizeof(PiPiezo));
|
|
if(!pNew)
|
|
{
|
|
Tcl_SetResult(pTcl,"Out of memory for PiPiezo Driver",NULL);
|
|
return NULL;
|
|
}
|
|
memset(pNew,0,sizeof(PiPiezo));
|
|
|
|
/* connection parameters */
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"Computer",TCL_GLOBAL_ONLY);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find serial port server host name",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->hostname = strdup(pPar);
|
|
|
|
pPar = NULL;
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"port",TCL_GLOBAL_ONLY);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find serial port server port adress",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetInt(pTcl,pPar,&iVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert port adress to integer",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->iPort = iVal;
|
|
|
|
pPar = NULL;
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"channel",TCL_GLOBAL_ONLY);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find serial port server channel adress",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetInt(pTcl,pPar,&iVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert channel number to integer",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->iChannel = iVal;
|
|
pPar = NULL;
|
|
|
|
pPar = (char *)Tcl_GetVar2(pTcl,pArray,"motor",TCL_GLOBAL_ONLY);
|
|
if(!pPar)
|
|
{
|
|
Tcl_SetResult(pTcl,"Failed to find motor number",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
iRet = Tcl_GetInt(pTcl,pPar,&iVal);
|
|
if(iRet != TCL_OK)
|
|
{
|
|
Tcl_SetResult(pTcl," Failed to convert motor number to integer",NULL);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
pNew->iMotor = iVal;
|
|
|
|
pNew->fUpper = 12000.;
|
|
pNew->fLower = -2000.;
|
|
|
|
/* 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);
|
|
KillPiPiezo(pNew);
|
|
return NULL;
|
|
}
|
|
|
|
/* 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;
|
|
pNew->KillPrivate = KillPiPiezo;
|
|
|
|
/* success */
|
|
return (MotorDriver *)pNew;
|
|
}
|