linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
sicspsi/el734hp.c

848 lines
23 KiB
C
Raw Permalink Blame History

/*------------------------------------------------------------------------
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 <time.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;
int posCount;
int runCount;
char errorReply[80];
time_t valueExpiry;
} 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
#define BADCOUNT -15
/*-------------------------------------------------------------------
This reruns the command when an emergency stop has been detected.
This ought to fix the problem that there is still another *ES in the
line even after releasing the emergency stop button
--------------------------------------------------------------------*/
static int transactEL734(prs232 self, void *send, int sendLen,
void *reply, int replylen)
{
int status, len, oldTimeout;
char *pReply = NULL;
char rubbish[2024];
/*
try to read away rubbish on the line first
*/
oldTimeout = getRS232Timeout(self);
setRS232Timeout(self, 0);
if (availableRS232(self)) {
len = sizeof(rubbish);
readRS232(self, rubbish, &len);
}
setRS232Timeout(self, oldTimeout);
/*
Actually do something. Some controllers send one erroneus *ES when the
emergency stop had been released. Therefore we believe an emergency stop
message only at the second try. This is the logic below. Some also send
more \r\n style rubbish on emergency stops too. This is what we try to
read away in the while loop below too.
*/
status = transactRS232(self, send, sendLen, reply, replylen);
if (status >= 1) {
pReply = (char *) reply;
if (strstr(pReply, "*ES") != NULL) {
while(availableRS232(self)) {
len = sizeof(rubbish);
readRS232(self, rubbish, &len);
}
status = transactRS232(self, send, sendLen, reply, replylen);
if (status >= 1) {
return 1;
}
} else {
return 1;
}
} else {
return status;
}
return 0; /* not reached */
}
/*-------------------------------------------------------------------
If we get an empty reply when we expect some response data, then, maybe,
the controller has sent us "\r*ES\r". Therefore this checks on
empty messages if there is some more, i.e: the *ES
--------------------------------------------------------------------*/
static void checkEmpty(pEL734Driv self, char *pReply, int *replylen)
{
int oldTimeout;
/*
if (strlen(pReply) < 1) {
oldTimeout = getRS232Timeout(self->controller);
setRS232Timeout(self->controller, 0);
if (availableRS232(self->controller)) {
readRS232(self->controller, pReply, replylen);
}
setRS232Timeout(self->controller, oldTimeout);
}
*/
if (strlen(pReply) < 1) {
readRS232(self->controller, pReply, replylen);
/* printf("checkEmpty read %s\n", pReply); */
}
}
/*--------------------------------------------------------------------*/
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 if (strstr(pReply, "*ms") != NULL) {
self->errorCode = BADSTP;
} else {
strlcpy(self->errorReply, pReply, 79);
self->errorCode = BADUNKNOWN;
}
return 0;
}
/*---------------------------------------------------------------------*/
static int EL734GetPos(void *pData, float *fPos)
{
pEL734Driv self = NULL;
int status, replyLen;
char pCommand[50], pReply[80];
self = (pEL734Driv) pData;
assert(self);
if (time(NULL) < self->valueExpiry) {
*fPos = self->lastValue;
return OKOK;
}
snprintf(pCommand, 79, "u %d\r", self->iMotor);
status = transactEL734(self->controller, pCommand, strlen(pCommand),
pReply, 79);
if (status != 1) {
self->errorCode = status;
return HWFault;
}
replyLen = 79;
checkEmpty(self, pReply, &replyLen);
if (!checkResponse(self, pReply)) {
return HWFault;
}
sscanf(pReply, "%f", fPos);
self->lastValue = *fPos;
self->valueExpiry = time(NULL) + 3;
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 = 3;
snprintf(pCommand, 79, "p %d %.3f\r", self->iMotor, fValue);
status = transactEL734(self->controller, pCommand, strlen(pCommand),
pReply, 79);
if (status != 1) {
self->errorCode = status;
return HWFault;
}
if (!checkResponse(self, pReply)) {
return HWFault;
}
self->posCount = 0;
self->runCount = 0;
return OKOK;
}
/*-----------------------------------------------------------------------*/
static int decodeMSR(pEL734Driv self, int msr)
{
if (msr == 0) {
if (self->posCount > 0 || self->runCount > 0) {
self->errorCode = BADCOUNT;
return HWPosFault;
}
/*
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 = POSFAULT;
return HWPosFault;
} else if (self->oredMsr & 0x1000) {
self->errorCode = BADCUSHION;
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 {
self->errorCode = BADUNKNOWN;
return HWFault; /* should not happen */
}
} else {
/*
we are still tugging along ............
*/
if (msr & 0x80) {
self->runCount++;
} else if (msr & 0x20) {
self->errorCode = HILIM;
return HWFault;
} else if (msr & 0x10) {
self->errorCode = LOWLIM;
return HWFault;
} else if (msr & 0x1000) {
self->errorCode = BADCUSHION;
return HWFault;
} else if (msr & 0x40) {
self->errorCode = BADSTP;
return HWFault;
} else if (msr & 0x200) {
self->posCount++;
} else if (msr & 0x100) {
self->errorCode = POSFAULT;
return HWFault;
} else if (msr & 0x400) {
self->errorCode = POSFAULT;
return HWFault;
}
return HWBusy;
}
}
/*------------------------------------------------------------------------*/
static int EL734Status(void *pData)
{
pEL734Driv self = NULL;
int status, msr, replyLen = 79;
char pCommand[50], pReply[80];
self = (pEL734Driv) pData;
assert(self);
snprintf(pCommand, 79, "msr %d\r", self->iMotor);
status = transactEL734(self->controller, pCommand, strlen(pCommand),
pReply, 79);
if (status < 0) {
self->errorCode = status;
return HWFault;
}
replyLen = 79;
checkEmpty(self, pReply, &replyLen);
if (!checkResponse(self, pReply)) {
return HWFault;
}
sscanf(pReply, "%x", &msr);
self->oredMsr |= msr;
self->valueExpiry = -1;
return decodeMSR(self, msr);
}
/*----------------------------------------------------------------------*/
static void EL734Error(void *pData, int *iCode, char *error, int errLen)
{
pEL734Driv self = NULL;
char pBueffel[132];
self = (pEL734Driv) pData;
assert(self);
*iCode = self->errorCode;
switch (*iCode) {
case BADADR:
strlcpy(error, "Bad address", errLen);
break;
case BADBSY:
strlcpy(error, "Motor still busy", errLen);
break;
case BADCMD:
strlcpy(error, "Bad command", errLen);
break;
case BADLOC:
strlcpy(error, "Motor controller is on local", errLen);
break;
case BADPAR:
strlcpy(error, "Bad parameter", errLen);
break;
case BADRNG:
strlcpy(error, "Bad range", errLen);
break;
case BADUNKNOWN:
snprintf(pBueffel, 131, "Unknown response: %s", self->errorReply);
strlcpy(error, pBueffel, errLen);
break;
case BADSTP:
strlcpy(error, "Motor is switched off at motor controller", errLen);
break;
case BADEMERG:
strlcpy(error, "Emergency stop is engaged, please release", errLen);
break;
case LOWLIM:
strlcpy(error, "Crashed into lower limit switch", errLen);
break;
case HILIM:
strlcpy(error, "Crashed into upper limit switch", errLen);
break;
case RUNFAULT:
strlcpy(error, "Run fault detected", errLen);
break;
case POSFAULT:
strlcpy(error, "Positioning fault detected", errLen);
break;
case BADCUSHION:
strlcpy(error, "Air cushion problem", errLen);
break;
case BADCOUNT:
snprintf(pBueffel, 131, "%d RunFaults, %d PosFaults",
self->runCount, self->posCount);
strlcpy(error, pBueffel, errLen);
break;
default:
getRS232Error(*iCode, error, errLen);
break;
}
}
/*----------------------------------------------------------------------*/
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 = transactEL734(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 EL734Fix(void *pData, int iCode, float fValue)
{
pEL734Driv self = NULL;
int status, msr, i, len = 49;
char pCommand[50], pReply[80];
self = (pEL734Driv) pData;
assert(self);
switch (iCode) {
case BADADR:
case BADCMD:
case BADPAR:
return MOTREDO;
case BADBSY:
EL734Halt(pData);
SicsWait(1);
return MOTREDO;
case TIMEOUT:
for (i = 0; i < 3; i++) {
len = 49;
status = readRS232TillTerm(self->controller, pReply, &len);
if (status == 1) {
return MOTREDO;
}
}
/*
If nothing can be read, the only fixable cause is a network breakdown
Try to fix this. If this does not work: give up
*/
closeRS232(self->controller);
SicsWait(60);
status = initRS232(self->controller);
if (status != 1) {
return MOTFAIL;
} else {
return MOTREDO;
}
break;
case BADUNKNOWN:
if (availableRS232(self->controller)) {
len = 79;
readRS232TillTerm(self->controller, pReply, &len);
return MOTREDO;
}
return MOTFAIL;
break;
case BADLOC:
snprintf(pCommand, 49, "RMT 1\r");
transactEL734(self->controller, pCommand, strlen(pCommand), pReply,
79);
snprintf(pCommand, 49, "ECHO 0\r");
transactEL734(self->controller, pCommand, strlen(pCommand), pReply,
79);
return MOTREDO;
case NOTCONNECTED:
initRS232(self->controller);
return MOTREDO;
case RUNFAULT:
case POSFAULT:
return MOTREDO;
case BADCOUNT:
self->runCount = 0;
self->posCount = 0;
return MOTOK;
case BADSEND:
/*
network problem: try to reopen connection
*/
closeRS232(self->controller);
SicsWait(60);
status = initRS232(self->controller);
if (status != 1) {
return MOTFAIL;
} else {
return MOTREDO;
}
}
return MOTFAIL;
}
/*--------------------------------------------------------------------*/
static int EL734GetPar(void *pData, char *name, float *fValue)
{
pEL734Driv self = NULL;
int status, replyLen = 79;
char pCommand[50], pReply[80];
self = (pEL734Driv) pData;
assert(self);
if (strcmp(name, "speed") == 0) {
snprintf(pCommand, 79, "J %d\r", self->iMotor);
status = transactEL734(self->controller, pCommand, strlen(pCommand),
pReply, 79);
if (status != 1) {
self->errorCode = status;
return 0;
}
checkEmpty(self, pReply, &replyLen);
if (!checkResponse(self, pReply)) {
return 0;
}
sscanf(pReply, "%f", fValue);
return 1;
}
return 0;
}
/*--------------------------------------------------------------------*/
static int EL734SetPar(void *pData, SConnection * pCon,
char *name, float newValue)
{
pEL734Driv self = NULL;
int status;
char pCommand[50], pReply[80];
self = (pEL734Driv) pData;
assert(self);
pCommand[0] = '\0';
if (strcmp(name, "speed") == 0) {
snprintf(pCommand, 79, "J %d %d\r", self->iMotor, (int) newValue);
} else if (strcmp(name, "forceupper") == 0) {
if (!SCMatchRights(pCon, usMugger)) {
return 0;
}
self->fUpper = newValue;
snprintf(pCommand, 79, "H %d %8.3f %8.3f\r", self->iMotor,
self->fLower, self->fUpper);
} else if (strcmp(name, "forcelower") == 0) {
if (!SCMatchRights(pCon, usMugger)) {
return 0;
}
self->fLower = newValue;
snprintf(pCommand, 79, "H %d %8.3f %8.3f\r", self->iMotor,
self->fLower, self->fUpper);
} else if (strcmp(name, "forcepos") == 0) {
if (!SCMatchRights(pCon, usMugger)) {
return 0;
}
snprintf(pCommand, 79, "U %d %8.3f\r", self->iMotor, newValue);
}
if (strlen(pCommand) > 1) {
status = transactEL734(self->controller, pCommand, strlen(pCommand),
pReply, 79);
if (status != 1) {
self->errorCode = status;
return 0;
}
if (!checkResponse(self, pReply)) {
snprintf(pCommand, 79, "ERROR %s while setting parameter", pReply);
SCWrite(pCon, pCommand, eError);
return 0;
}
return 1;
}
return 0;
}
/*--------------------------------------------------------------------*/
static void EL734List(void *self, char *name, SConnection * pCon)
{
float value;
char pBueffel[256];
EL734GetPar(self, "speed", &value);
snprintf(pBueffel, 255, "%s speed = %f", name, value);
SCWrite(pCon, pBueffel, eValue);
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, i, success;
prs232 controller = NULL;
char pCommand[50], pReply[80], pError[255];
/*
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;
pNew->oredMsr = 3;
/*
connection will already have been set up, read limits
*/
snprintf(pCommand, 49, "h %d\r", pNew->iMotor);
success = 0;
for (i = 0; i < 3; i++) {
status = transactEL734(pNew->controller, pCommand, strlen(pCommand),
pReply, 79);
if (status != 1) {
getRS232Error(status, pReply, 79);
snprintf(pError, 255, "ERROR: %s", pReply);
SCWrite(pCon, pError, eError);
closeRS232(pNew->controller);
SicsWait(60);
initRS232(pNew->controller);
} else {
if (checkResponse(pNew, pReply)) {
if (sscanf(pReply, "%f %f", &pNew->fLower, &pNew->fUpper) != 2) {
snprintf(pError, 255,
"ERROR: received shitty HW limit response from SICS: %s",
pReply);
SCWrite(pCon, pError, eError);
} else {
success = 1;
break;
}
}
}
}
if (success == 0) {
SCWrite(pCon,
"ERROR: invalid response when reading HW limits, defaulting..",
eError);
pNew->fLower = -180.;
pNew->fUpper = 180.;
}
snprintf(pError, 255, "EL734 returned HW-limits of: %s", pReply);
SCWrite(pCon, pError, eError);
return (MotorDriver *) pNew;
}
/*================ a 1000 scaled motor for SANSLI =================*/
static int EL734TRun(void *pData, float fValue)
{
return EL734Run(pData, fValue * 1000);
}
/*-----------------------------------------------------------------*/
static int EL734TGetPos(void *pData, float *fPos)
{
int status;
status = EL734GetPos(pData, fPos);
*fPos /= 1000;
return status;
}
/*-------------------------------------------------------------------*/
MotorDriver *CreateEL734HPT(SConnection * pCon, int argc, char *argv[])
{
MotorDriver *pDriv = NULL;
pDriv = CreateEL734HP(pCon, argc, argv);
if (pDriv != NULL) {
pDriv->GetPosition = EL734TGetPos;
pDriv->RunTo = EL734TRun;
pDriv->fLower /= 1000.;
pDriv->fUpper /= 1000.;
}
return pDriv;
}
/*======================================================================
A version which ignores the air cushion error for TRICS. Not recommended for
general use!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
But HW cannot be fixed.... as the electronics guys are not there.......
========================================================================*/
static int EL734FixAir(void *pData, int iCode, float fValue)
{
pEL734Driv self = NULL;
int status, msr, i, len = 49;
char pCommand[50], pReply[80];
self = (pEL734Driv) pData;
assert(self);
switch (iCode) {
/*
air cushion error is really OK.....
*/
case BADCUSHION:
return MOTOK;
case BADADR:
case BADCMD:
case BADPAR:
return MOTREDO;
case BADBSY:
EL734Halt(pData);
SicsWait(1);
return MOTREDO;
case TIMEOUT:
for (i = 0; i < 3; i++) {
len = 49;
status = readRS232TillTerm(self->controller, pReply, &len);
if (status == 1) {
return MOTREDO;
}
}
/*
If nothing can be read, the only fixable cause is a network breakdown
Try to fix this. If this does not work: give up
*/
closeRS232(self->controller);
SicsWait(60);
status = initRS232(self->controller);
if (status != 1) {
return MOTFAIL;
} else {
return MOTREDO;
}
break;
case BADUNKNOWN:
if (availableRS232(self->controller)) {
len = 79;
readRS232TillTerm(self->controller, pReply, &len);
return MOTREDO;
}
return MOTFAIL;
break;
case BADLOC:
snprintf(pCommand, 49, "RMT 1\r");
transactEL734(self->controller, pCommand, strlen(pCommand), pReply,
79);
snprintf(pCommand, 49, "ECHO 0\r");
transactEL734(self->controller, pCommand, strlen(pCommand), pReply,
79);
return MOTREDO;
case NOTCONNECTED:
initRS232(self->controller);
return MOTREDO;
case RUNFAULT:
case POSFAULT:
return MOTREDO;
case BADCOUNT:
self->runCount = 0;
self->posCount = 0;
return MOTOK;
case BADSEND:
/*
network problem: try to reopen connection
*/
closeRS232(self->controller);
SicsWait(60);
status = initRS232(self->controller);
if (status != 1) {
return MOTFAIL;
} else {
return MOTREDO;
}
}
return MOTFAIL;
}
/*-------------------------------------------------------------------------*/
MotorDriver *CreateEL734Air(SConnection * pCon, int argc, char *argv[])
{
MotorDriver *pDriv = NULL;
pDriv = CreateEL734HP(pCon, argc, argv);
if (pDriv != NULL) {
pDriv->TryAndFixIt = EL734FixAir;
}
return pDriv;
}
Reference in New Issue View Git Blame Copy Permalink