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

- Fixes to el734hp.c for handling of emergency stop

Browse Source 
- Added waiting 5 sec after driving A6 to tas code
This commit is contained in:
koennecke
2005-03-16 07:59:56 +00:00
parent ed3c13e47c
commit 7bf31b710b
4 changed files with 135 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
el734hp.c
View File

@ -77,6 +77,66 @@
#define POSFAULT -13
#define BADCUSHION -14
#define BADCOUNT -15
/*-------------------------------------------------------------------
This reruns the command when an emergency stop has been detected.
This ougth 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;
/*
try to read away rubbish on the line first
*/
oldTimeout = getRS232Timeout(self);
setRS232Timeout(self,0);
if(availableRS232(self)){
len = replylen;
readRS232(self,reply,&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.
*/
status = transactRS232(self,send,sendLen,reply,replylen);
if(status == 1){
pReply = (char *)reply;
if(strstr(pReply,"*ES") != NULL){
if(availableRS232(self)){
len = replylen;
readRS232TillTerm(self,reply,&len);
}
return transactRS232(self,send,sendLen,reply,replylen);
} else {
return status;
}
} else {
return status;
}
}
/*-------------------------------------------------------------------
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);
}
}
/*--------------------------------------------------------------------*/
static int checkResponse(pEL734Driv self, char *pReply){
/*
@ -111,7 +171,7 @@ static int checkResponse(pEL734Driv self, char *pReply){
/*---------------------------------------------------------------------*/
static int EL734GetPos(void *pData, float *fPos){
pEL734Driv self = NULL;
int status;
int status, replyLen;
char pCommand[50],pReply[80];
self = (pEL734Driv)pData;
@ -123,12 +183,16 @@ static int EL734GetPos(void *pData, float *fPos){
}
snprintf(pCommand,79,"u %d\r",self->iMotor);
status = transactRS232(self->controller,pCommand,strlen(pCommand),
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;
}
@ -148,7 +212,7 @@ static int EL734Run(void *pData,float fValue){
self->oredMsr = 0;
snprintf(pCommand,79,"p %d %.3f\r",self->iMotor,fValue);
status = transactRS232(self->controller,pCommand,strlen(pCommand),
status = transactEL734(self->controller,pCommand,strlen(pCommand),
pReply,79);
if(status != 1){
self->errorCode = status;
@ -189,9 +253,6 @@ static int decodeMSR(pEL734Driv self, int msr){
} 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;
@ -225,19 +286,22 @@ static int decodeMSR(pEL734Driv self, int msr){
/*------------------------------------------------------------------------*/
static int EL734Status(void *pData){
pEL734Driv self = NULL;
int status, msr;
int status, msr, replyLen = 79;
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),
status = transactEL734(self->controller,pCommand,strlen(pCommand),
pReply,79);
if(status < 0){
self->errorCode = status;
return HWFault;
}
checkEmpty(self,pReply,&replyLen);
if(!checkResponse(self,pReply)){
return HWFault;
}
@ -345,9 +409,17 @@ static int EL734Fix(void *pData, int iCode, float fValue){
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");
transactRS232(self->controller,pCommand,strlen(pCommand),pReply,79);
transactEL734(self->controller,pCommand,strlen(pCommand),pReply,79);
return MOTREDO;
case NOTCONNECTED:
initRS232(self->controller);
@ -384,7 +456,7 @@ static int EL734Halt(void *pData){
assert(self);
snprintf(pCommand,79,"s %d\r",self->iMotor);
status = transactRS232(self->controller,pCommand,strlen(pCommand),
status = transactEL734(self->controller,pCommand,strlen(pCommand),
pReply,79);
if(status != 1){
self->errorCode = status;
@ -399,19 +471,22 @@ static int EL734Halt(void *pData){
static int EL734GetPar(void *pData, char *name,
float *fValue){
pEL734Driv self = NULL;
int status;
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 = transactRS232(self->controller,pCommand,strlen(pCommand),
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;
}
@ -429,15 +504,32 @@ static int EL734SetPar(void *pData, SConnection *pCon,
self = (pEL734Driv)pData;
assert(self);
pCommand[0] ='\0';
if(strcmp(name,"speed") == 0){
snprintf(pCommand,79,"J %d %d\r",self->iMotor,(int)newValue);
status = transactRS232(self->controller,pCommand,strlen(pCommand),
} 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);
}
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;
@ -512,14 +604,14 @@ MotorDriver *CreateEL734HP(SConnection *pCon, int argc, char *argv[]){
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);
success = 0;
for(i = 0; i < 3; i++){
status = transactRS232(pNew->controller, pCommand,strlen(pCommand),
status = transactEL734(pNew->controller, pCommand,strlen(pCommand),
pReply,79);
if(status != 1){
getRS232Error(status,pReply,79);