linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
sicspsi/hardsup/el734tcl.c
koennecke b3ac7dff3f - Adapted indenation to new agreed upon system
2009-02-13 09:01:24 +00:00

584 lines
16 KiB
C
Raw Permalink Blame History

/*--------------------------------------------------------------------------
Some code to make EL734 motors as used at SINQ available in TCL.
Just a wrapper around David Maden's motor control routines.
You are free to use and modify this software for noncommercial
usage.
No warranties or liabilities of any kind taken by me or my employer
Mark Koennecke June 1996
----------------------------------------------------------------------------*/
#include "sinq_prototypes.h"
/* #include <timers.h>*/
#include <tcl.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "rs232c_def.h"
#include "el734_def.h"
#define INACTIVE 999.8
#define MOTACURRACY 0.02
#define False 0
#define True 1
typedef struct {
float fUpper; /* upper limit */
float fLower; /* Lower Limit */
int iFix; /* fixed, unfixed flag */
float fSoftZero; /* SW-zero point */
float fSoftUpper; /* software upper boundary */
float fSoftLower; /* " lower " */
int iLowFlag, iUpFlag, iZeroFlag; /*activity flags */
void *pData; /* EL734 open struct */
} EL734st;
EXTERN int EL734Action(ClientData pDat, Tcl_Interp * i, int a,
char *argv[]);
static void EL734Error2Text(char *pBuffer, int errcode);
/*---------------------------------------------------------------------------
Tcl has a high niceness level. It deletes a command properly when
exiting, reinitializing etc. I use this facility to kill off the
motor initialised in EL734.
---------------------------------------------------------------------------*/
EXTERN void EL734Murder(ClientData pData)
{
EL734st *pTa = (EL734st *) pData;
EL734_Close(&(pTa->pData));
free(pData);
}
/*----------------------------------------------------------------------------
EL734 is the main entry point for this stuff. It connects to a motor
and, on success, creates a new command with the name of the motor.
Syntax:
EL734 name host port channel index
---------------------------------------------------------------------------*/
int EL734(ClientData clientData, Tcl_Interp * interp,
int argc, char *argv[])
{
int iRet;
EL734st *pEL734 = NULL;
int iPort, iChannel, iMotor;
char *pErr = NULL;
char pBueffel[80];
/* check arguments */
if (argc < 6) {
Tcl_AppendResult(interp,
" Insufficient arguments: EL734 name host port channel index",
(char *) NULL);
return TCL_ERROR;
}
/* convert arguments */
iRet = Tcl_GetInt(interp, argv[3], &iPort);
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need integer value for port", (char *) NULL);
return iRet;
}
iRet = Tcl_GetInt(interp, argv[4], &iChannel);
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need integer value for channel",
(char *) NULL);
return iRet;
}
iRet = Tcl_GetInt(interp, argv[5], &iMotor);
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need integer value for motor",
(char *) NULL);
return iRet;
}
/* make a new pointer, initialise EL734st */
pEL734 = (EL734st *) malloc(sizeof(EL734st));
if (pEL734 == NULL) {
Tcl_AppendResult(interp, "No memory in EL734", NULL);
return TCL_ERROR;
}
pEL734->iFix = False;
pEL734->fSoftZero = INACTIVE + 1;
pEL734->fSoftUpper = INACTIVE + 1.;
pEL734->fSoftLower = -INACTIVE - 1.;
pEL734->iZeroFlag = False;
pEL734->iLowFlag = False;
pEL734->iUpFlag = False;
/* open the motor, finally */
iRet =
EL734_Open(&(pEL734->pData), argv[2], iPort, iChannel, iMotor,
"STPMC EL734");
if (iRet) { /* success */
/* figure out motor limits */
EL734_GetLimits(&(pEL734->pData), &(pEL734->fLower),
&(pEL734->fUpper));
/* handle TCL, create new command: the motor */
Tcl_CreateCommand(interp, strdup(argv[1]), EL734Action,
(ClientData) pEL734, EL734Murder);
Tcl_AppendResult(interp, strdup(argv[1]), (char *) NULL);
return TCL_OK;
} else {
EL734_ErrInfo(&pErr, &iPort, &iChannel, &iMotor);
EL734Error2Text(pBueffel, iPort);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
free(pEL734);
return TCL_ERROR;
}
}
/*---------------------------------------------------------------------------
CheckPos checks a position and converts it to a real position.
Returns TCL_ERROR on mistake, TCL_OK else
---------------------------------------------------------------------------*/
static int CheckPos(Tcl_Interp * interp, EL734st * pData,
float fRequested, float *fDrive)
{
float fPos;
char pBueffel[132];
/* fixed ? */
if (pData->iFix) {
Tcl_AppendResult(interp, "Motor fixed", NULL);
return TCL_ERROR;
}
/* check against SW-boundaries */
if (pData->iUpFlag) {
if (fRequested > pData->fSoftUpper) {
sprintf(pBueffel,
"Requested position: %f violates SW-boundary %f",
fRequested, pData->fSoftUpper);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_ERROR;
}
}
if (pData->iLowFlag) {
if (fRequested < pData->fSoftLower) {
sprintf(pBueffel,
"Requested position: %f violates SW-boundary %f",
fRequested, pData->fSoftLower);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_ERROR;
}
}
/* correct for zero point */
if (pData->iZeroFlag) {
fPos = fRequested - pData->fSoftZero;
} else {
fPos = fRequested;
}
/* check HW-boundaries */
if ((fPos < pData->fLower) || (fPos > pData->fUpper)) {
sprintf(pBueffel, " %f outside limits %f %f",
fPos, pData->fLower, pData->fUpper);
Tcl_AppendResult(interp, "Requested position: ",
pBueffel, (char *) NULL);
return TCL_ERROR;
}
*fDrive = fPos;
return TCL_OK;
}
/* -------------------------------------------------------------------------
fucking standard library missing functionality!!!!!!!!!!!!!!!!
---------------------------------------------------------------------------*/
static float absf(float x)
{
if (x < .0)
return -x;
else
return x;
}
/*--------------------------------------------------------------------------
EL734 Action is the routine where commands send to the motor will
end up.
Syntax:
motor lim shows motor limits
motor dr val drives the motor to val
motor run val set the motor in motion, without waiting
for completion
motor pos shows motor position
motor fix fixes a motor at a position
motor unfix unfixes a fixed motor
motor zero val set a software zero point
motor upper val sets a software upper limit
motor lower val sets a software lower limit
----------------------------------------------------------------------------*/
EXTERN int EL734Action(ClientData clientData, Tcl_Interp * interp,
int argc, char *argv[])
{
EL734st *pData = (EL734st *) clientData;
char pBueffel[80];
char *pErr = NULL;
int iRet, iMSR, iOMSR, iFPC, iFRC, iSS;
float fPos, fNpos;
double dPos;
int i;
struct RS__RplyStruct *pReply = NULL;
/* check parameters */
if (argc < 2) {
Tcl_AppendResult(interp,
"Usage: motor and either dr, pos, hlim slim run zero up lo",
(char *) NULL);
return TCL_ERROR;
}
if (pData == NULL) {
Tcl_AppendResult(interp, "Motor data lost!!!!!!!!", (char *) NULL);
return TCL_ERROR;
}
/* check for HW-lim */
if (strcmp(argv[1], "hlim") == 0) {
sprintf(pBueffel, " %f %f", pData->fLower, pData->fUpper);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_OK;
}
/* check for SW-lim */
if (strcmp(argv[1], "slim") == 0) {
sprintf(pBueffel, " %f %f", pData->fSoftLower, pData->fSoftUpper);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_OK;
}
/* fix & unfix */
if (strcmp(argv[1], "fix") == 0) {
pData->iFix = True;
return TCL_OK;
}
if (strcmp(argv[1], "unfix") == 0) {
pData->iFix = False;
return TCL_OK;
}
/* reset */
if (strcmp(argv[1], "reset") == 0) {
pData->iFix = False;
pData->iLowFlag = False;
pData->iUpFlag = False;
pData->iZeroFlag = False;
return TCL_OK;
}
/* check for pos */
if (strcmp(argv[1], "pos") == 0) {
iRet = EL734_GetStatus(&(pData->pData),
&iMSR, &iOMSR, &iFPC, &iFRC, &iSS, &fPos);
if (!iRet) {
EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
EL734Error2Text(pBueffel, iMSR);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_ERROR;
} else {
if (pData->iZeroFlag) {
fPos += pData->fSoftZero;
}
sprintf(pBueffel, " %f", fPos);
Tcl_AppendResult(interp, pBueffel, NULL);
return TCL_OK;
}
}
/* zero point */
if (strcmp(argv[1], "zero") == 0) {
/* check for zero already been defined */
if (pData->iZeroFlag) {
Tcl_AppendResult(interp,
"Request to set new zero point rejected.",
" Use reset before new definition. ",
" I'll get confused otherwise ", NULL);
return TCL_ERROR;
}
/* get the new position */
if (argc < 3) {
Tcl_AppendResult(interp, "Usage: motor zero val", NULL);
return TCL_ERROR;
}
iRet = Tcl_GetDouble(interp, argv[2], &dPos);
fNpos = dPos;
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need float value for new zeropint",
(char *) NULL);
return iRet;
}
pData->fSoftZero = -fNpos;
pData->iZeroFlag = True;
return TCL_OK;
}
/* upper SW-limit */
if (strcmp(argv[1], "up") == 0) {
/* get the new position */
if (argc < 3) {
Tcl_AppendResult(interp, "Usage: motor up val", NULL);
return TCL_ERROR;
}
iRet = Tcl_GetDouble(interp, argv[2], &dPos);
fNpos = dPos;
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need float value for new upper limit",
(char *) NULL);
return iRet;
}
pData->fSoftUpper = fNpos;
pData->iUpFlag = True;
return TCL_OK;
}
/* lower SW-limit */
if (strcmp(argv[1], "lo") == 0) {
/* get the new position */
if (argc < 3) {
Tcl_AppendResult(interp, "Usage: motor lo val", NULL);
return TCL_ERROR;
}
iRet = Tcl_GetDouble(interp, argv[2], &dPos);
fNpos = dPos;
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need float value for new lower limit",
(char *) NULL);
return iRet;
}
pData->fSoftLower = fNpos;
pData->iLowFlag = True;
return TCL_OK;
}
/* this is most important: dr for Drive */
if (strcmp(argv[1], "dr") == 0) {
/* get the new position */
if (argc < 3) {
Tcl_AppendResult(interp, "Usage: motor dr val", NULL);
return TCL_ERROR;
}
iRet = Tcl_GetDouble(interp, argv[2], &dPos);
fNpos = dPos;
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need value to drive to", (char *) NULL);
return iRet;
}
/* check if compatible with limits */
if (CheckPos(interp, pData, fNpos, &fPos) == TCL_ERROR)
return TCL_ERROR;
/* finally move */
iRet = EL734_MoveWait(&(pData->pData), fPos, &iOMSR,
&iFPC, &iFRC, &fNpos);
/* 99.99999999999% of all code is error checking */
if (!iRet) {
EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
EL734Error2Text(pBueffel, iMSR);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_ERROR;
}
/* check if driving has been done */
if (absf(fPos - fNpos) > MOTACURRACY) {
Tcl_AppendResult(interp,
" Motor error: inacurate driving!",
" Probably something serious is wrong ",
" Check the fucking hardware ", NULL);
return TCL_ERROR;
}
return TCL_OK;
}
/* this is most important: run for Driving without waiting */
if (strcmp(argv[1], "run") == 0) {
/* get the new position */
if (argc < 3) {
Tcl_AppendResult(interp, "Usage: motor run val", NULL);
return TCL_ERROR;
}
iRet = Tcl_GetDouble(interp, argv[2], &dPos);
fNpos = dPos;
if (iRet == TCL_ERROR) {
Tcl_AppendResult(interp, "Need value to run for", (char *) NULL);
return iRet;
}
/* check if compatible with limits */
if (CheckPos(interp, pData, fNpos, &fPos) == TCL_ERROR)
return TCL_ERROR;
/* finally move */
iRet = EL734_MoveNoWait(&(pData->pData), fPos);
/* 99.99999999999% of all code is error checking */
if (!iRet) {
EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
EL734Error2Text(pBueffel, iMSR);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_ERROR;
}
return TCL_OK;
}
/* the dangerous, undocumented expert command: com:
sends something directly to the motor. All args following
com will be concatenated in one string, closed with \r
and send to the motor . A GetReply will be invoked in order
to yield a return value. Usage by normal motor users strictly
discouraged.
*/
if (strcmp(argv[1], "com") == 0) {
strcpy(pBueffel, argv[2]);
for (i = 3; i < argc; i++) {
strcat(pBueffel, " ");
strcat(pBueffel, argv[i]);
}
sprintf(pBueffel, "%s\r", pBueffel);
iRet = EL734_SendCmnds(&(pData->pData), pBueffel, NULL);
if (!iRet) {
EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
EL734Error2Text(pBueffel, iMSR);
Tcl_AppendResult(interp, pBueffel, (char *) NULL);
return TCL_ERROR;
}
/* fetch reply */
pReply =
(struct RS__RplyStruct *) EL734_GetReply(&(pData->pData), NULL);
while (pReply != NULL) {
Tcl_AppendElement(interp, pReply->rply);
pReply =
(struct RS__RplyStruct *) EL734_GetReply(&(pData->pData),
pReply);
}
return TCL_OK;
}
/* if we end here an unknown command has been sent */
Tcl_AppendResult(interp,
"Usage: motor and either dr, run,zero, pos, hlim"
"slim up low reset fix unfix", (char *) NULL);
return TCL_ERROR;
}
/*---------------------------------------------------------------------------
EL734Error2Text converts between an EL734 error code to text
-----------------------------------------------------------------------------*/
void EL734Error2Text(char *pBuffer, int iErr)
{
switch (iErr) {
case -28:
strcpy(pBuffer, "EL734__BAD_ADR");
break;
case -8:
strcpy(pBuffer, "EL734__BAD_BIND");
break;
case -30:
strcpy(pBuffer, "EL734__BAD_BSY");
break;
case -3:
strcpy(pBuffer, "EL734__BAD_CMD");
break;
case -9:
strcpy(pBuffer, "EL734__BAD_CONNECT");
break;
case -23:
strcpy(pBuffer, "EL734__BAD_FLUSH");
break;
case -6:
strcpy(pBuffer, "EL734__BAD_HOST");
break;
case -10:
strcpy(pBuffer, "EL734__BAD_ID");
break;
case -5:
strcpy(pBuffer, "EL734__BAD_ILLG");
break;
case -2:
strcpy(pBuffer, "EL734__BAD_LOC");
break;
case -11:
strcpy(pBuffer, "EL734__BAD_MALLOC");
break;
case -21:
strcpy(pBuffer, "EL734__BAD_NOT_BCD");
break;
case -4:
strcpy(pBuffer, "EL734__BAD_OFL");
break;
case -29:
strcpy(pBuffer, "EL734__BAD_PAR");
break;
case -17:
strcpy(pBuffer, "EL734__BAD_RECV");
break;
case -19:
strcpy(pBuffer, "EL734__BAD_RECV_NET");
break;
case -18:
strcpy(pBuffer, "EL734__BAD_RECV_PIPE");
break;
case -20:
strcpy(pBuffer, "EL734__BAD_RECV_UNKN");
break;
case -22:
strcpy(pBuffer, "EL734__BAD_RECVLEN");
break;
case -24:
strcpy(pBuffer, "EL734__BAD_RECV1");
break;
case -26:
strcpy(pBuffer, "EL734__BAD_RECV1_NET");
break;
case -25:
strcpy(pBuffer, "EL734__BAD_RECV1_PIPE");
break;
case -27:
strcpy(pBuffer, "EL734__BAD_RNG");
break;
case -13:
strcpy(pBuffer, "EL734__BAD_SEND");
break;
case -14:
strcpy(pBuffer, "EL734__BAD_SEND_PIPE");
break;
case -15:
strcpy(pBuffer, "EL734__BAD_SEND_NET");
break;
case -16:
strcpy(pBuffer, "EL734__BAD_SEND_UNKN");
break;
case -12:
strcpy(pBuffer, "EL734__BAD_SENDLEN");
break;
case -7:
strcpy(pBuffer, "EL734__BAD_SOCKET");
break;
case -1:
strcpy(pBuffer, "EL734__BAD_TMO");
break;
default:
strcpy(pBuffer, "Unknown EL734 error");
break;
}
}
Reference in New Issue View Git Blame Copy Permalink