- New drivers for EL737 and EL734 high performance
- Changes to makefiles
This commit is contained in:
cvs
447
el734hp.c
Normal file
447
el734hp.c
Normal file
@ -0,0 +1,447 @@
|
||||
/*------------------------------------------------------------------------
|
||||
This is another driver for the PSI EL734 motor controllers as used
|
||||
at SINQ. The idea is that this one is performing better then the
|
||||
other one which uses David Madens SerPortServer program. The
|
||||
speedup is gained through:
|
||||
- direct access to the controller
|
||||
- reduction in the amount of data transferred
|
||||
- in status: send first, read only when data available. Cannot do this:
|
||||
up to 8 motors share a controller: I may get a status response for
|
||||
a wrong motor or overload the controller with to many confusing status
|
||||
requests.
|
||||
|
||||
copyright: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, July 2003
|
||||
-----------------------------------------------------------------------*/
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
#include <string.h>
|
||||
#include <fortify.h>
|
||||
#include <sics.h>
|
||||
#include <modriv.h>
|
||||
#include <rs232controller.h>
|
||||
|
||||
|
||||
/*-----------------------------------------------------------------------
|
||||
The motor driver structure. Please note that the first set of fields has
|
||||
be identical with the fields of AbstractModriv in ../modriv.h
|
||||
------------------------------------------------------------------------*/
|
||||
typedef struct __MoDriv {
|
||||
/* 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);
|
||||
|
||||
|
||||
/* EL-734 specific fields */
|
||||
prs232 controller;
|
||||
int iMotor;
|
||||
float lastValue;
|
||||
int errorCode;
|
||||
int oredMsr;
|
||||
} EL734Driv, *pEL734Driv;
|
||||
/*------------------- error codes ----------------------------------*/
|
||||
#define BADADR -1
|
||||
#define BADBSY -2
|
||||
#define BADCMD -3
|
||||
#define BADLOC -4
|
||||
#define BADPAR -5
|
||||
#define BADRNG -6
|
||||
#define BADUNKNOWN -7
|
||||
#define BADSTP -8
|
||||
#define BADEMERG -9
|
||||
#define LOWLIM -10
|
||||
#define HILIM -11
|
||||
#define RUNFAULT -12
|
||||
#define POSFAULT -13
|
||||
#define BADCUSHION -14
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int checkResponse(pEL734Driv self, char *pReply){
|
||||
/*
|
||||
error messages start with ?, if none we are done
|
||||
*/
|
||||
if(strstr(pReply,"?") == NULL && strstr(pReply,"*") == NULL){
|
||||
return 1;
|
||||
}
|
||||
strtolower(pReply);
|
||||
if(strstr(pReply,"?adr") != NULL){
|
||||
self->errorCode = BADADR;
|
||||
} else if(strstr(pReply,"?bsy") != NULL){
|
||||
self->errorCode = BADBSY;
|
||||
} else if(strstr(pReply,"?cmd") != NULL){
|
||||
self->errorCode = BADCMD;
|
||||
} else if(strstr(pReply,"?loc") != NULL){
|
||||
self->errorCode = BADLOC;
|
||||
} else if(strstr(pReply,"?par") != NULL){
|
||||
self->errorCode = BADPAR;
|
||||
} else if(strstr(pReply,"?rng") != NULL){
|
||||
self->errorCode = BADRNG;
|
||||
}else if(strstr(pReply,"*es") != NULL){
|
||||
self->errorCode = BADEMERG;
|
||||
} else {
|
||||
self->errorCode = BADUNKNOWN;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static int EL734GetPos(void *pData, float *fPos){
|
||||
pEL734Driv self = NULL;
|
||||
int status;
|
||||
char pCommand[50],pReply[80];
|
||||
|
||||
self = (pEL734Driv)pData;
|
||||
assert(self);
|
||||
|
||||
snprintf(pCommand,79,"u %d\r",self->iMotor);
|
||||
status = transactRS232(self->controller,pCommand,strlen(pCommand),
|
||||
pReply,79);
|
||||
if(status != 1){
|
||||
self->errorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
if(!checkResponse(self,pReply)){
|
||||
return HWFault;
|
||||
}
|
||||
sscanf(pReply,"%f",fPos);
|
||||
self->lastValue = *fPos;
|
||||
return OKOK;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int EL734Run(void *pData,float fValue){
|
||||
pEL734Driv self = NULL;
|
||||
int status;
|
||||
char pCommand[50],pReply[80];
|
||||
|
||||
self = (pEL734Driv)pData;
|
||||
assert(self);
|
||||
|
||||
self->oredMsr = 0;
|
||||
snprintf(pCommand,79,"p %d %3.f\r",self->iMotor,fValue);
|
||||
status = transactRS232(self->controller,pCommand,strlen(pCommand),
|
||||
pReply,79);
|
||||
if(status != 1){
|
||||
self->errorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
if(!checkResponse(self,pReply)){
|
||||
return HWFault;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int decodeMSR(pEL734Driv self, int msr){
|
||||
if(msr == 0){
|
||||
/*
|
||||
we are done: check ored_msr for troubles
|
||||
*/
|
||||
if(self->oredMsr & 0x2){
|
||||
return HWIdle;
|
||||
} else if(self->oredMsr & 0x10){
|
||||
self->errorCode = LOWLIM;
|
||||
return HWFault;
|
||||
} else if(self->oredMsr & 0x20){
|
||||
self->errorCode = HILIM;
|
||||
return HWFault;
|
||||
} else if(self->oredMsr & 0x80){
|
||||
self->errorCode = RUNFAULT;
|
||||
return HWPosFault;
|
||||
} else if(self->oredMsr & 0x200){
|
||||
self->errorCode = RUNFAULT;
|
||||
return HWPosFault;
|
||||
} else if(self->oredMsr & 0x1000){
|
||||
self->errorCode = BADCUSHION;
|
||||
return HWFault;
|
||||
} else if(self->oredMsr & 0x8){
|
||||
self->errorCode = BADSTP;
|
||||
return HWFault;
|
||||
} else if(self->oredMsr & 0x40) {
|
||||
self->errorCode = BADSTP;
|
||||
return HWFault;
|
||||
} else if(self->oredMsr & 0x100){
|
||||
self->errorCode = POSFAULT;
|
||||
return HWFault;
|
||||
} else if(self->oredMsr & 0x400){
|
||||
self->errorCode = POSFAULT;
|
||||
return HWFault;
|
||||
}
|
||||
} else {
|
||||
/*
|
||||
we are still tugging along ............
|
||||
*/
|
||||
if(msr & 0x10){
|
||||
self->errorCode = LOWLIM;
|
||||
return HWFault;
|
||||
} else if(msr & 0x20){
|
||||
self->errorCode = HILIM;
|
||||
return HWFault;
|
||||
} else if(msr & 0x80){
|
||||
self->errorCode = RUNFAULT;
|
||||
return HWPosFault;
|
||||
} else if(self->oredMsr & 0x200){
|
||||
self->errorCode = RUNFAULT;
|
||||
return HWPosFault;
|
||||
} else if(msr & 0x1000){
|
||||
self->errorCode = BADCUSHION;
|
||||
return HWFault;
|
||||
} else if(msr & 0x8){
|
||||
self->errorCode = BADSTP;
|
||||
return HWFault;
|
||||
} else if(msr & 0x40) {
|
||||
self->errorCode = BADSTP;
|
||||
return HWFault;
|
||||
} else if(msr & 0x100){
|
||||
self->errorCode = POSFAULT;
|
||||
return HWFault;
|
||||
} else if(msr & 0x400){
|
||||
self->errorCode = POSFAULT;
|
||||
return HWFault;
|
||||
} else {
|
||||
return HWBusy;
|
||||
}
|
||||
}
|
||||
}
|
||||
/*------------------------------------------------------------------------*/
|
||||
static int EL734Status(void *pData){
|
||||
pEL734Driv self = NULL;
|
||||
int status, msr;
|
||||
char pCommand[50],pReply[80];
|
||||
|
||||
self = (pEL734Driv)pData;
|
||||
assert(self);
|
||||
|
||||
snprintf(pCommand,79,"msr %d\r",self->iMotor);
|
||||
status = transactRS232(self->controller,pCommand,strlen(pCommand),
|
||||
pReply,79);
|
||||
if(status < 0){
|
||||
self->errorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
if(!checkResponse(self,pReply)){
|
||||
return HWFault;
|
||||
}
|
||||
sscanf(pReply,"%x",&msr);
|
||||
self->oredMsr |= msr;
|
||||
return decodeMSR(self,msr);
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static void EL734Error(void *pData, int *iCode, char *error, int errLen){
|
||||
pEL734Driv self = NULL;
|
||||
self = (pEL734Driv)pData;
|
||||
assert(self);
|
||||
|
||||
*iCode = self->errorCode;
|
||||
switch(*iCode){
|
||||
case BADADR:
|
||||
strncpy(error,"Bad address",errLen);
|
||||
break;
|
||||
case BADBSY:
|
||||
strncpy(error,"Motor still busy",errLen);
|
||||
break;
|
||||
case BADCMD:
|
||||
strncpy(error,"Bad command",errLen);
|
||||
break;
|
||||
case BADLOC:
|
||||
strncpy(error,"Motor controller is on local",errLen);
|
||||
break;
|
||||
case BADPAR:
|
||||
strncpy(error,"Bad parameter",errLen);
|
||||
break;
|
||||
case BADRNG:
|
||||
strncpy(error,"Bad range",errLen);
|
||||
break;
|
||||
case BADUNKNOWN:
|
||||
strncpy(error,"Unknown error condition",errLen);
|
||||
break;
|
||||
case BADSTP:
|
||||
strncpy(error,"Motor is stopped",errLen);
|
||||
break;
|
||||
case BADEMERG:
|
||||
strncpy(error,"Emergency stop is engaged",errLen);
|
||||
break;
|
||||
case LOWLIM:
|
||||
strncpy(error,"Crashed into lower limit switch",errLen);
|
||||
break;
|
||||
case HILIM:
|
||||
strncpy(error,"Crashed into upper limit switch",errLen);
|
||||
break;
|
||||
case RUNFAULT:
|
||||
strncpy(error,"Run fault detected",errLen);
|
||||
break;
|
||||
case POSFAULT:
|
||||
strncpy(error,"Positioning fault detected",errLen);
|
||||
break;
|
||||
case BADCUSHION:
|
||||
strncpy(error,"Air cushion problem",errLen);
|
||||
break;
|
||||
default:
|
||||
getRS232Error(*iCode,error,errLen);
|
||||
break;
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int EL734Fix(void *pData, int iCode, float fValue){
|
||||
pEL734Driv self = NULL;
|
||||
int status, msr;
|
||||
char pCommand[50],pReply[80];
|
||||
|
||||
self = (pEL734Driv)pData;
|
||||
assert(self);
|
||||
|
||||
switch(iCode){
|
||||
case BADADR:
|
||||
case BADCMD:
|
||||
case TIMEOUT:
|
||||
case BADPAR:
|
||||
case BADBSY:
|
||||
return MOTREDO;
|
||||
case BADLOC:
|
||||
snprintf(pCommand,49,"RMT 1\r");
|
||||
transactRS232(self->controller,pCommand,strlen(pCommand),pReply,79);
|
||||
return MOTREDO;
|
||||
case NOTCONNECTED:
|
||||
initRS232(self->controller);
|
||||
return MOTREDO;
|
||||
case RUNFAULT:
|
||||
return MOTREDO;
|
||||
}
|
||||
return MOTFAIL;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int EL734Halt(void *pData){
|
||||
pEL734Driv self = NULL;
|
||||
int status;
|
||||
char pCommand[50],pReply[80];
|
||||
|
||||
self = (pEL734Driv)pData;
|
||||
assert(self);
|
||||
|
||||
snprintf(pCommand,79,"s %d\r",self->iMotor);
|
||||
status = transactRS232(self->controller,pCommand,strlen(pCommand),
|
||||
pReply,79);
|
||||
if(status != 1){
|
||||
self->errorCode = status;
|
||||
return 0;
|
||||
}
|
||||
if(!checkResponse(self,pReply)){
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int EL734GetPar(void *self, char *name,
|
||||
float *fValue){
|
||||
return 0;
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int EL734SetPar(void *self, SConnection *pCon,
|
||||
char *name, float newValue){
|
||||
return 0;
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static void EL734List(void *self, char *name, SConnection *pCon){
|
||||
return;
|
||||
}
|
||||
/*---------------------------------------------------------------------*/
|
||||
static void KillEL734(void *pData){
|
||||
/*
|
||||
the controller is owned by the controller object and will be
|
||||
deleted when that object is removed
|
||||
*/
|
||||
return;
|
||||
}
|
||||
/*------------------------------------------------------------------*/
|
||||
MotorDriver *CreateEL734HP(SConnection *pCon, int argc, char *argv[]){
|
||||
pEL734Driv pNew = NULL;
|
||||
int motor, status;
|
||||
prs232 controller = NULL;
|
||||
char pCommand[50],pReply[80];
|
||||
|
||||
/*
|
||||
check arguments
|
||||
*/
|
||||
if(argc < 2){
|
||||
SCWrite(pCon,"ERROR: not enough arguments to create EL734HP driver",
|
||||
eError);
|
||||
return NULL;
|
||||
}
|
||||
controller = (prs232)FindCommandData(pServ->pSics,argv[0],
|
||||
"RS232 Controller");
|
||||
if(!controller){
|
||||
SCWrite(pCon,"ERROR: motor controller not found",eError);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
motor = atoi(argv[1]);
|
||||
if(motor < 0 || motor > 12){
|
||||
SCWrite(pCon,"ERROR: invalid motor number",eError);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*
|
||||
allocate and initialize data structure
|
||||
*/
|
||||
pNew = (pEL734Driv)malloc(sizeof(EL734Driv));
|
||||
if(!pNew){
|
||||
SCWrite(pCon,"ERROR: no memory to allocate motor driver",
|
||||
eError);
|
||||
return NULL;
|
||||
}
|
||||
memset(pNew,0,sizeof(EL734Driv));
|
||||
pNew->GetPosition = EL734GetPos;
|
||||
pNew->RunTo = EL734Run;
|
||||
pNew->GetStatus = EL734Status;
|
||||
pNew->GetError = EL734Error;
|
||||
pNew->TryAndFixIt = EL734Fix;
|
||||
pNew->Halt = EL734Halt;
|
||||
pNew->GetDriverPar = EL734GetPar;
|
||||
pNew->SetDriverPar = EL734SetPar;
|
||||
pNew->ListDriverPar = EL734List;
|
||||
pNew->KillPrivate = KillEL734;
|
||||
pNew->controller = controller;
|
||||
pNew->iMotor = motor;
|
||||
|
||||
/*
|
||||
connection will already have been set up, read limits
|
||||
*/
|
||||
snprintf(pCommand,49,"h %d\r",pNew->iMotor);
|
||||
status = transactRS232(pNew->controller, pCommand,strlen(pCommand),
|
||||
pReply,79);
|
||||
if(!status){
|
||||
SCWrite(pCon,"ERROR: failed to read HW limits, defaulting..",eError);
|
||||
pNew->fLower = -180.;
|
||||
pNew->fUpper = 180.;
|
||||
} else {
|
||||
if(checkResponse(pNew,pReply)){
|
||||
sscanf(pReply,"%f %f",&pNew->fLower,&pNew->fUpper);
|
||||
} else {
|
||||
SCWrite(pCon,
|
||||
"ERROR: invalid response when reading HW limits, defaulting..",
|
||||
eError);
|
||||
pNew->fLower = -180.;
|
||||
pNew->fUpper = 180.;
|
||||
}
|
||||
}
|
||||
return (MotorDriver *)pNew;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user