375 lines
9.2 KiB
C
375 lines
9.2 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) {
|
|
strlcpy(pError, "Timeout at serial line", iErrLen);
|
|
return;
|
|
}
|
|
if (*iCode == PICOMERR) {
|
|
strlcpy(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;
|
|
}
|