- 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
389
pipiezo.c
Normal file
389
pipiezo.c
Normal file
@ -0,0 +1,389 @@
|
||||
/*--------------------------------------------------------------------------
|
||||
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 <tcl.h>
|
||||
#include "fortify.h"
|
||||
#include "sics.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 = 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 = 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 = 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 = 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;
|
||||
}
|
||||
Reference in New Issue
Block a user