linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
661df3916658b199e4fa632e860321b2bb75e8ed
sicspsi/el734dc.c
cvs 064ec37e9a - Rearranged directory structure for forking out ANSTO 
- Refactored site specific stuff into a site module
- PSI specific stuff is now in the PSI directory.
- The old version has been tagged with pre-ansto
2003-06-20 10:18:47 +00:00

941 lines
28 KiB
C
Raw Blame History

/*-------------------------------------------------------------------------
A motor driver for EL734 DC motors as used at SinQ
Mark Koennecke, November 1996
Original code foe EL734 stepper, modified for DC motors, the
11-June-1997 Mark Koennecke
Copyright:
Labor fuer Neutronenstreuung
Paul Scherrer Institut
CH-5423 Villigen-PSI
The authors hereby grant permission to use, copy, modify, distribute,
and license this software and its documentation for any purpose, provided
that existing copyright notices are retained in all copies and that this
notice is included verbatim in any distributions. No written agreement,
license, or royalty fee is required for any of the authorized uses.
Modifications to this software may be copyrighted by their authors
and need not follow the licensing terms described here, provided that
the new terms are clearly indicated on the first page of each file where
they apply.
IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE
IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
MODIFICATIONS.
------------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <fortify.h>
#include <sics.h>
#include "modriv.h"
#include "hardsup/sinq_prototypes.h"
#include "hardsup/rs232c_def.h"
#include "hardsup/el734_def.h"
#include "hardsup/el734fix.h"
#include <bit.h>
#include <splitter.h>
#include <servlog.h>
static int EL734EncodeMSR(char *text, int iLen,
int iMSR, int iOMSR, int iFP, int iFR);
static int EL734AnalyzeMSR(int iMSR, int iOMSR);
/* addional error codes for Status-things */
#define MSRBUSY -40
#define MSRONLIMIT -41
#define MSRRUNFAULT -42
#define MSRPOSFAULT -43
#define MSRDEADCUSHION -44
#define MSRHALT -45
#define MSRSTOP -46
#define MSROK -47
#define MSRREF -48
#define MSRFAULT -49
/*-----------------------------------------------------------------------
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 */
int iPort;
char *hostname;
int iChannel;
int iMotor;
void *EL734struct;
int iMSR;
} EL734Driv;
/* --------------------------------------------------------------------------*/
static int GetPos(void *self, float *fData)
{
EL734Driv *pDriv;
float fPos;
int iRet, iMSR, iOMSR, iFRC,iFPC, iSS;
assert(self);
pDriv = (EL734Driv *)self;
iRet = EL734_GetStatus(&(pDriv->EL734struct),
&iMSR,
&iOMSR,
&iFPC,
&iFRC,
&iSS,
&fPos);
if(iMSR != 0)
{
pDriv->iMSR = iMSR;
}
*fData = fPos;
if(iRet != 1)
{
return HWFault;
}
else
return OKOK;
}
/*--------------------------------------------------------------------------*/
static int Run(void *self, float fNew)
{
EL734Driv *pDriv;
int iRet;
assert(self);
pDriv = (EL734Driv *)self;
iRet = EL734_MoveNoWait (&(pDriv->EL734struct), fNew);
if(iRet == 1)
{
return OKOK;
}
else
{
return HWFault;
}
}
/*---------------------------------------------------------------------------
EL734Error2Text converts between an EL734 error code to text
-----------------------------------------------------------------------------*/
static void EL734Error2Text(char *pBuffer, int iErr)
{
strcpy(pBuffer,"ERROR: HW:");
switch(iErr)
{
case EL734__BAD_ADR:
strcat(pBuffer,"EL734__BAD_ADR");
break;
case EL734__BAD_BIND:
strcat(pBuffer,"EL734__BAD_BIND");
break;
case EL734__BAD_CMD:
strcat(pBuffer,"EL734__BAD_CMD");
break;
case EL734__BAD_CONNECT:
strcat(pBuffer,"EL734__BAD_CONNECT");
break;
case EL734__BAD_FLUSH:
strcat(pBuffer,"EL734__BAD_FLUSH");
break;
case EL734__BAD_HOST:
strcat(pBuffer,"EL734__BAD_HOST");
break;
case EL734__BAD_ID:
strcat(pBuffer,"EL734__BAD_ID");
break;
case EL734__BAD_ILLG:
strcat(pBuffer,"EL734__BAD_ILLG");
break;
case EL734__BAD_LOC:
strcat(pBuffer,"EL734__BAD_LOC");
break;
case EL734__BAD_MALLOC:
strcat(pBuffer,"EL734__BAD_MALLOC");
break;
case EL734__BAD_NOT_BCD:
strcat(pBuffer,"EL734__BAD_NOT_BCD");
break;
case EL734__BAD_OFL:
strcat(pBuffer,"EL734__BAD_OFL");
break;
case EL734__BAD_PAR:
strcat(pBuffer,"EL734__BAD_PAR");
break;
case EL734__BAD_RECV:
strcat(pBuffer,"EL734__BAD_RECV");
break;
case EL734__BAD_RECV_NET:
strcat(pBuffer,"EL734__BAD_RECV_NET");
break;
case EL734__BAD_RECV_PIPE:
strcat(pBuffer,"EL734__BAD_RECV_PIPE");
break;
case EL734__BAD_RECV_UNKN:
strcat(pBuffer,"EL734__BAD_RECV_UNKN");
break;
case EL734__BAD_RECVLEN:
strcat(pBuffer,"EL734__BAD_RECVLEN");
break;
case EL734__BAD_RECV1:
strcat(pBuffer,"EL734__BAD_RECV1");
break;
case EL734__BAD_RECV1_NET:
strcat(pBuffer,"EL734__BAD_RECV1_NET");
break;
case EL734__BAD_RECV1_PIPE:
strcat(pBuffer,"EL734__BAD_RECV1_PIPE");
break;
case EL734__BAD_RNG:
strcat(pBuffer,"EL734__BAD_RNG");
break;
case EL734__BAD_SEND:
strcat(pBuffer,"EL734__BAD_SEND");
break;
case EL734__BAD_SEND_PIPE:
strcat(pBuffer,"EL734__BAD_SEND_PIPE");
break;
case EL734__BAD_SEND_NET:
strcat(pBuffer,"EL734__BAD_SEND_NET");
break;
case EL734__BAD_SEND_UNKN:
strcat(pBuffer,"EL734__BAD_SEND_UNKN");
break;
case EL734__BAD_SENDLEN:
strcat(pBuffer,"EL734__BAD_SENDLEN");
break;
case EL734__BAD_SOCKET:
strcat(pBuffer,"EL734__BAD_SOCKET");
break;
case EL734__BAD_TMO:
strcat(pBuffer,"EL734__BAD_TMO");
break;
case EL734__FORCED_CLOSED:
strcat(pBuffer,"EL734__FORCED_CLOSED");
break;
case EL734__BAD_STP:
strcat(pBuffer,"EL734__BAD_STP");
break;
case EL734__EMERG_STOP:
strcat(pBuffer,"EL734__EMERG_STOP");
break;
case EL734__NOT_OPEN:
strcat(pBuffer,"EL734__NOT_OPEN");
break;
case EL734__BAD_ASYNSRV:
strcat(pBuffer,"EL734__BAD_ASYNSRV");
break;
default:
sprintf(pBuffer,"Unknown EL734 error %d", iErr);
break;
}
}
/*-------------------------------------------------------------------------*/
static void GetErr(void *self, int *iCode, char *buffer, int iBufLen)
{
EL734Driv *pDriv;
char pBueffel[512];
int iMSR, iOMSR, iSS;
int iRet, iFPC, iFRC;
int iErr;
float fPos;
char *pErr;
assert(self);
/* get EL734 error codes */
pDriv = (EL734Driv *)self;
EL734_ErrInfo(&pErr,&iMSR,&iOMSR, &iSS);
if(iMSR != 0)
{
EL734Error2Text(pBueffel,iMSR);
strncpy(buffer,pBueffel,(iBufLen-1));
*iCode = iMSR;
return;
}
else
{ /* check status flag for addional errors */
iRet = EL734_GetStatus(&(pDriv->EL734struct),
&iMSR,
&iOMSR,
&iFPC,
&iFRC,
&iSS,
&fPos);
if(iRet != 1)
{ /* failure on this one, this has to be handled */
EL734_ErrInfo(&pErr,&iMSR,&iOMSR, &iSS);
EL734Error2Text(pBueffel,iMSR);
strncpy(buffer,pBueffel,(iBufLen-1));
*iCode = iMSR;
return;
}
else
{
/* we really come down to looking at status flags */
*iCode = EL734EncodeMSR(buffer,iBufLen,iMSR, iOMSR,iFPC,iFRC);
}
}
}
/* ------------------------------------------------------------------------
Types of errors possible on EL734:
Network error: Try reopening connection and redo command.
Than there are problems which might have to do with a dodgy RS232,
resend command may help
Some things cannot be fixed.
*/
static int FixError(void *self, int iError, float fNew)
{
EL734Driv *pDriv;
int iRet;
char pBueffel[512];
int iMSR, iOMSR, iSS;
float fPos;
assert(self);
pDriv = (EL734Driv *)self;
sprintf(pBueffel,"EL734 : %s %d %d %d Problem:",pDriv->hostname,
pDriv->iPort, pDriv->iChannel, pDriv->iMotor);
/* get & check MSR flags */
/* check for codes */
switch(iError)
{
case 0: /* no error at all */
return MOTOK;
case EL734__BAD_ID: /* ID */
case EL734__BAD_ADR: /* ADR */
case EL734__BAD_CMD: /* CMD */
case EL734__BAD_ILLG: /* ILLG */
case EL734__BAD_PAR: /* PAR */
case EL734__BAD_TMO:
SICSLogWrite(pBueffel,eHWError);
SICSLogWrite("BAD Command or dodgy RS-232",eHWError);
return MOTREDO;
case EL734__EMERG_STOP:
return MOTFAIL;
case EL734__BAD_RNG: /* RNG */
case MSRONLIMIT:
SICSLogWrite(pBueffel,eHWError);
SICSLogWrite("Out of Range",eHWError);
return MOTFAIL;
case EL734__BAD_STP:
return MOTFAIL;
break;
case MSRBUSY:
return MOTREDO;
case MSRRUNFAULT:
SICSLogWrite(pBueffel,eHWError);
SICSLogWrite("------ RUN Fault in Controller ---- ",eHWError);
return MOTFAIL;
case MSRPOSFAULT:
SICSLogWrite(pBueffel,eHWError);
SICSLogWrite("------ POS Fault in Controller ---- ",eHWError);
return MOTFAIL;
case MSRDEADCUSHION:
SICSLogWrite(pBueffel,eHWError);
SICSLogWrite("------ Air cushion Fault in Controller ---- ",eHWError);
return MOTFAIL;
case MSRFAULT:
return MOTFAIL;
case MSRHALT:
case MSRSTOP:
return MOTFAIL;
case EL734__FORCED_CLOSED:
case EL734__NOT_OPEN:
iRet = EL734_Open(&(pDriv->EL734struct),pDriv->hostname,
pDriv->iPort,pDriv->iChannel,
pDriv->iMotor,"DCMC EL734");
if(iRet != 1)
{
return MOTFAIL;
}
else
{
return MOTREDO;
}
break;
case EL734__BAD_LOC: /* LO2 */
case EL734__BAD_OFL:
EL734_Close(&(pDriv->EL734struct),0);
iRet = EL734_Open(&(pDriv->EL734struct),pDriv->hostname,
pDriv->iPort,pDriv->iChannel,
pDriv->iMotor,"DCMC EL734");
if(iRet != 1)
{
return MOTFAIL;
}
else
{
return MOTREDO;
}
break;
/* case EL734__BAD_ASYNSRV:
EL734_Close(&(pDriv->EL734struct),1);
return MOTREDO;
break;
*/ default:
SICSLogWrite(pBueffel,eHWError);
SICSLogWrite("Network problem, trying to reopen",eHWError);
EL734_Close(&(pDriv->EL734struct),1);
iRet = EL734_Open(&(pDriv->EL734struct),pDriv->hostname,
pDriv->iPort,pDriv->iChannel,
pDriv->iMotor,"DCMC EL734");
if(iRet != 1)
{
return MOTFAIL;
}
else
{
return MOTREDO;
}
}
}
/*--------------------------------------------------------------------------*/
static int Halt(void *self)
{
EL734Driv *pDriv;
int iRet;
char pBueffel[80];
assert(self);
pDriv = (EL734Driv *)self;
iRet = EL734_Stop(&(pDriv->EL734struct));
if(iRet != 1)
{
return OKOK;
}
return HWFault;
}
/*--------------------------------------------------------------------------*/
static int GetStat(void *self)
{
EL734Driv *pDriv;
float fPos;
int iRet, iMSR, iOMSR, iFRC,iFPC, iSS;
int eRet;
int iTest;
char pBueffel[80];
assert(self);
pDriv = (EL734Driv *)self;
iRet = EL734_GetStatus(&(pDriv->EL734struct),
&iMSR,
&iOMSR,
&iFPC,
&iFRC,
&iSS,
&fPos);
if(iRet != 1)
{
return HWFault;
}
if(iMSR != 0)
{
pDriv->iMSR = iMSR;
}
iTest = EL734AnalyzeMSR(iMSR,iOMSR);
switch(iTest)
{
case MSRDEADCUSHION:
case MSRONLIMIT:
case MSRREF:
case MSRHALT:
case MSRSTOP:
return HWFault;
break;
case MSRRUNFAULT:
case MSRPOSFAULT:
return HWPosFault;
break;
case MSRBUSY:
return HWBusy;
break;
case MSRFAULT:
return HWWarn;
break;
default:
return HWIdle;
break;
}
}
/*--------------------------------------------------------------------------*/
extern void KillEL734(void *pdata); /* from el734driv.c */
/*---------------------------------------------------------------------------*/
static EL734Driv *MakeEL734DC(char *hostname, int iPort, int iChannel,
int iMotor)
{
EL734Driv *pDriv = NULL;
int iError;
char pBueffel[80];
char *pErr;
int iRet;
int iDummy;
/* create a new struct */
pDriv = (EL734Driv *)malloc(sizeof(EL734Driv));
if(!pDriv)
{
return NULL;
}
memset(pDriv,0,sizeof(EL734Driv));
/* fill in some of the data entered */
pDriv->hostname = strdup(hostname);
pDriv->iPort = iPort;
pDriv->iChannel = iChannel;
pDriv->iMotor = iMotor;
pDriv->name = strdup("EL734");
/* try opening the motor */
iRet = EL734_Open(&(pDriv->EL734struct), hostname,iPort,
iChannel,iMotor,"DCMC EL734");
if(iRet != 1)
{
EL734_ErrInfo(&pErr,&iError,&iRet, &iDummy);
KillEL734((void *)pDriv);
return NULL;
}
/* now get the limits */
EL734_GetLimits(&(pDriv->EL734struct),&(pDriv->fLower),
&(pDriv->fUpper));
/* initialise the function pointers */
pDriv->GetPosition = GetPos;
pDriv->RunTo = Run;
pDriv->GetError = GetErr;
pDriv->GetStatus = GetStat;
pDriv->Halt = Halt;
pDriv->TryAndFixIt = FixError;
pDriv->KillPrivate = KillEL734;
return pDriv;
}
/*--------------------------------------------------------------------------
interpreting the driver parameters is up to the driver, this below
inplements just this
*/
MotorDriver *CreateEL734DC(SConnection *pCon, int argc, char *argv[])
{
EL734Driv *pDriv = NULL;
TokenList *pList = NULL;
TokenList *pCurrent;
char *hostname;
int iPort, iChannel, iMotor;
char pBueffel[512];
assert(pCon);
/* split arguments */
pList = SplitArguments(argc,argv);
if(!pList)
{
SCWrite(pCon,"Error parsing arguments",eError);
return NULL;
}
/* first must be hostname */
pCurrent = pList;
if(pCurrent->Type != eText)
{
sprintf(pBueffel,"EL734DC: Expected hostname but got --> %s <--",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return NULL;
}
hostname = pCurrent->text;
/* next should be port */
pCurrent = pCurrent->pNext;
if(!pCurrent)
{
SCWrite(pCon,"EL734DC: Insufficient number of arguments",eError);
DeleteTokenList(pList);
return NULL;
}
if(pCurrent->Type != eInt)
{
sprintf(pBueffel,"EL734DC: Expected Integer as Port number, got --> %s <--",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return NULL;
}
iPort = pCurrent->iVal;
/* next should be Channel number */
pCurrent = pCurrent->pNext;
if(!pCurrent)
{
SCWrite(pCon,"EL734DC: Insufficient number of arguments",eError);
DeleteTokenList(pList);
return NULL;
}
if(pCurrent->Type != eInt)
{
sprintf(pBueffel,"EL734DC: Expected Integer as channel number, got --> %s <--",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return NULL;
}
iChannel = pCurrent->iVal;
/* finally motor number */
pCurrent = pCurrent->pNext;
if(!pCurrent)
{
SCWrite(pCon,"EL734DC: Insufficient number of arguments",eError);
DeleteTokenList(pList);
return NULL;
}
if(pCurrent->Type != eInt)
{
sprintf(pBueffel,"EL734DC: Expected Integer as motor number, got --> %s <--",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return NULL;
}
iMotor = pCurrent->iVal;
/* finally initialize driver */
pDriv = MakeEL734DC(hostname,iPort,iChannel,iMotor);
if(!pDriv)
{
SCWrite(pCon,"EL734DC: error opening motor, check adress",eError);
pDriv = NULL;
}
/* clean up */
DeleteTokenList(pList);
return (MotorDriver *)pDriv;
}
/*-------------------------------------------------------------------------
Stolen from David and modified to return an integer error code as well
*/
static int EL734EncodeMSR (char *text, int text_len,
int msr,
int ored_msr,
int fp_cntr,
int fr_cntr) {
int len;
char my_text[132];
char my_text_0[32];
int iRet = 0;
if (msr == 0) {
ored_msr = ored_msr & ~(MSR__BUSY); /* Zero "Busy" bit */
if (ored_msr == MSR__OK) {
StrJoin (text, text_len, "Status, MSR = Idle. Positioned OK.", "");
}else {
if ((ored_msr & MSR__OK) != 0) {
StrJoin (text, text_len, "Status, MSR = Idle. Positioned OK. ", "");
}else {
StrJoin (text, text_len, "Status, MSR = Idle. ", "");
}
if ((ored_msr & MSR__REF_OK) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Ref. Pos'n OK. ");
}
if ((ored_msr & MSR__LIM_ERR) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Limit Switch Problem. ");
iRet = MSRONLIMIT;
}
if ((ored_msr & MSR__AC_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Air-Cushion Error. ");
iRet = MSRDEADCUSHION;
}
if ((ored_msr & MSR__REF_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Ref. Pos'n Fail. ");
}
if ((ored_msr & MSR__POS_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Pos'n Fail. ");
iRet = MSRPOSFAULT;
}
if ((ored_msr & MSR__POS_FAULT) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
if (fp_cntr == 1) {
StrJoin (text, text_len, my_text, "1 Pos'n Fault. ");
}else {
sprintf (my_text_0, "%d Pos'n Faults. ", fp_cntr);
StrJoin (text, text_len, my_text, my_text_0);
}
}
if ((ored_msr & MSR__RUN_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Run Fail. ");
iRet = MSRRUNFAULT;
}
if ((ored_msr & MSR__RUN_FAULT) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
if (fr_cntr == 1) {
StrJoin (text, text_len, my_text, "1 Run Fault. ");
}else {
sprintf (my_text_0, "%d Run Faults. ", fr_cntr);
StrJoin (text, text_len, my_text, my_text_0);
}
}
if ((ored_msr & MSR__HALT) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Halt. ");
iRet = MSRHALT;
}
if ((ored_msr & MSR__HI_LIM) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Hit HiLim. ");
iRet = MSRONLIMIT;
}
if ((ored_msr & MSR__LO_LIM) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Hit LoLim. ");
iRet = MSRONLIMIT;
}
if ((ored_msr & MSR__STOPPED) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Stopped. ");
iRet = MSRSTOP;
}
}
}else if ((msr & ~(0x2fff)) != 0) {
StrJoin (text, text_len, "Status, MSR = ??", "");
}else {
sprintf (my_text, "%#x ", msr);
StrJoin (text, text_len, "Status, MSR = ", my_text);
if ((msr & MSR__LIM_ERR) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Limit Switch Problem/");
iRet = MSRONLIMIT;
}
if ((msr & MSR__AC_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Air-Cushion Error/");
iRet = MSRDEADCUSHION;
}
if ((msr & MSR__REF_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Ref. Pos'n Fail/");
}
if ((msr & MSR__POS_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Pos'n Fail/");
iRet = MSRPOSFAULT;
}
if ((msr & MSR__POS_FAULT) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Pos'n Fault/");
}
if ((msr & MSR__RUN_FAIL) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Run Fail/");
iRet = MSRRUNFAULT;
}
if ((msr & MSR__RUN_FAULT) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Run Fault/");
}
if ((msr & MSR__HALT) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Halt/");
iRet = MSRHALT;
}
if ((msr & MSR__HI_LIM) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Hit HiLim/");
iRet = MSRONLIMIT;
}
if ((msr & MSR__LO_LIM) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Hit LoLim/");
iRet = MSRONLIMIT;
}
if ((msr & MSR__STOPPED) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Stopped/");
iRet = MSRSTOP;
}
if ((msr & MSR__REF_OK) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Ref. Pos'n OK/");
}
if ((msr & MSR__OK) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "OK/");
}
if ((msr & MSR__BUSY) != 0) {
StrJoin (my_text, sizeof (my_text), text, "");
StrJoin (text, text_len, my_text, "Busy/");
}
len = strlen (text);
text[len-1] = '\0';
}
return iRet;
}
/*-------------------------------------------------------------------------*/
static int EL734AnalyzeMSR(int msr,int ored_msr)
{
int iRet = 0;
/* this means the motor is done */
if (msr == 0) {
ored_msr = ored_msr & ~(MSR__BUSY); /* Zero "Busy" bit */
if (ored_msr == MSR__OK) {
iRet = MSROK;
}else {
if ((ored_msr & MSR__OK) != 0) {
iRet = MSROK;
}else {
iRet = MSROK;
}
if ((ored_msr & MSR__REF_OK) != 0) {
iRet = MSROK;
}
if ((ored_msr & MSR__LIM_ERR) != 0) {
return MSRONLIMIT;
}
if ((ored_msr & MSR__AC_FAIL) != 0) {
return MSRDEADCUSHION;
}
if ((ored_msr & MSR__REF_FAIL) != 0) {
iRet = MSRREF;
}
if ((ored_msr & MSR__POS_FAIL) != 0) {
return MSRPOSFAULT;
}
if ((ored_msr & MSR__POS_FAULT) != 0) {
iRet = MSRFAULT;
}
if ((ored_msr & MSR__RUN_FAIL) != 0) {
return MSRRUNFAULT;
}
if ((ored_msr & MSR__RUN_FAULT) != 0) {
iRet = MSRFAULT;
}
if ((ored_msr & MSR__HALT) != 0) {
return MSRHALT;
}
if ((ored_msr & MSR__HI_LIM) != 0) {
return MSRONLIMIT;
}
if ((ored_msr & MSR__LO_LIM) != 0) {
return MSRONLIMIT;
}
if ((ored_msr & MSR__STOPPED) != 0) {
return MSRSTOP;
}
}
/* the motor is still fighting along */
}else if ((msr & ~(0x2fff)) != 0) {
iRet = MSROK;
}else {
if ((msr & MSR__LIM_ERR) != 0) {
return MSRONLIMIT;
}
if ((msr & MSR__AC_FAIL) != 0) {
return MSRDEADCUSHION;
}
if ((msr & MSR__REF_FAIL) != 0) {
iRet = MSRREF;
}
if ((msr & MSR__POS_FAIL) != 0) {
return MSRPOSFAULT;
}
if ((msr & MSR__POS_FAULT) != 0) {
iRet = MSRFAULT;
}
if ((msr & MSR__RUN_FAIL) != 0) {
return MSRRUNFAULT;
}
if ((msr & MSR__RUN_FAULT) != 0) {
iRet = MSRFAULT;
}
if ((msr & MSR__HALT) != 0) {
return MSRHALT;
}
if ((msr & MSR__HI_LIM) != 0) {
return MSRONLIMIT;
}
if ((msr & MSR__LO_LIM) != 0) {
return MSRONLIMIT;
}
if ((msr & MSR__STOPPED) != 0) {
return MSRSTOP;
}
if ((msr & MSR__REF_OK) != 0) {
iRet = MSROK;
}
if ((msr & MSR__OK) != 0) {
iRet = MSROK;
}
if ((msr & MSR__BUSY) != 0) {
iRet = MSRBUSY;
}
}
return iRet;
}
Reference in New Issue View Git Blame Copy Permalink