- 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
This commit is contained in:
cvs
940
el734dc.c
Normal file
940
el734dc.c
Normal file
@@ -0,0 +1,940 @@
|
||||
/*-------------------------------------------------------------------------
|
||||
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
Block a user