linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Adapted indenation to new agreed upon system

Browse Source 
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
2009-02-13 09:01:03 +00:00
parent 6c7bb14fad
commit eb72d5c486
151 changed files with 38234 additions and 38208 deletions
Download Patch File Download Diff File Expand all files Collapse all files

689
pipiezo.c
View File

@ -21,369 +21,354 @@
/*================== 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;
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;
}
static int PIPosition(void *pData, float *fPos)
{
pPiPiezo self = NULL;
int iRet;
char pCommand[20];
char pReply[80];
char *pPtr;
/* 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;
self = (pPiPiezo) pData;
assert(self);
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;
}
/* format command */
sprintf(pCommand, "%1.1dTO\r", self->iMotor);
/* 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;
/* 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);
}
}
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;
MotorDriver *MakePiPiezo(Tcl_Interp * pTcl, char *pArray)
{
pPiPiezo pNew = NULL;
int iRet, iVal, iTmo;
double dVal;
char *pPar = NULL;
char pCommand[20], pReply[40];
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;
}
/* 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;
}