diff --git a/site_ansto/motor_dmc2280.c b/site_ansto/motor_dmc2280.c index 99b569cf..e1c5d4b4 100644 --- a/site_ansto/motor_dmc2280.c +++ b/site_ansto/motor_dmc2280.c @@ -1,13 +1,14 @@ -/*------------------------------------------------------------------------ - Implements a SICS motor object with a MotorDriver interface. - The control and communications functions are implemented in a separate layer - in instrument/dmc2280.tcl - -Copyright: see file Copyright.txt - -Ferdi Franceschini November 2005 ------------------------------------------------------------------------*/ +/** \file motor_dmc2280.c + * \brief Driver for Galil DMC2280 motor controller. + * + * Implements a SICS motor object with a MotorDriver interface. + * + * Copyright: see file Copyright.txt + * + * Ferdi Franceschini November 2005 + */ #include +#include #include #include #include @@ -25,6 +26,13 @@ Ferdi Franceschini November 2005 #include "splint/splint_SCinter.h" */ +/*@-incondefs@*/ +/* XXX Should this also free pData */ +int readRS232(prs232 self, /*@out@*/void *data, /*@out@*/int *dataLen); +void KillRS232(/*@only@*/ void *pData); +/*@only@*/ Tcl_GetVar2(Tcl_Interp *interp, char *name1, char *name2, int flags); +/*@observer@*/ Tcl_Interp *InterpGetTcl(SicsInterp *pSics); +/*@+incondefs@*/ /*----------------------------------------------------------------------- The motor driver structure. Please note that the first set of fields has be identical with the fields of AbstractModriv in ../modriv.h @@ -59,21 +67,21 @@ typedef struct __MoDriv { int oredMsr; int lastValue; int iConfig; - char units[256]; /* physical units for axis */ - float speed; /* physical units per second */ - float maxSpeed; /* physical units per second */ - float accel; /* physical units per second^2 */ - float maxAccel; /* physical units per second^2 */ - float decel; /* physical units per second^2 */ - float maxDecel; /* physical units per second^2 */ + char units[256]; /**< physical units for axis */ + float speed; /**< physical units per second */ + float maxSpeed; /**< physical units per second */ + float accel; /**< physical units per second^2 */ + float maxAccel; /**< physical units per second^2 */ + float decel; /**< physical units per second^2 */ + float maxDecel; /**< physical units per second^2 */ char axisLabel; char lastCmd[1024]; - float home; /* home position for axis, default=0 */ - int motorHome; /* motor home position in steps */ - int stepsPerX; /* steps per physical unit */ - int abs_endcoder; /* Flag = 1 if there is an abs enc */ - int absEncHome; /* Home position in counts for abs enc */ - int cntsPerX; /* absolute encoder counts per physical unit */ + float home; /**< home position for axis, default=0 */ + int motorHome; /**< motor home position in steps */ + int stepsPerX; /**< steps per physical unit */ + int abs_endcoder; /**< Flag = 1 if there is an abs enc */ + int absEncHome; /**< Home position in counts for abs enc */ + int cntsPerX; /**< absolute encoder counts per physical unit */ } DMC2280Driv, *pDMC2280Driv; /*------------------- error codes ----------------------------------*/ #define BADADR -1 // Unknown host/port? @@ -142,7 +150,17 @@ static int motDecel(pDMC2280Driv self, float decel) { return motDecel; } -static int DMC2280ReadChar(pDMC2280Driv self, char *reply) { +/** \brief Reads a single character from the DMC2280 controller. + * + * On failure it sets the errorCode field in the motor's data structure + * \param self (rw) provides access to the motor's data structure + * \param *reply (w) the character read from the controller + * \return + * - SUCCESS + * - FAILURE + * \see SUCCESS FAILURE + */ +static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) { int i, status, retries=20, dataLen=1; for (i=0; icontroller, reply, &dataLen); @@ -159,11 +177,24 @@ static int DMC2280ReadChar(pDMC2280Driv self, char *reply) { } return FAILURE; } -/*---------------------------------------------------------------------*/ -/* First character returned by controller is + +/** \brief Sends a DMC2280 command to the motor controller. + * + * If the command fails it displays the DMC2280 error message to the client + * and writes it to the log file, also sets errorCode field in motor's + * data structure. + * + * \param self (rw) provides access to the motor's data structure + * \param *command (r) DMC2280 command + * \return + * - SUCCESS + * - FAILURE + * \see SUCCESS FAILURE + */ + /* First character returned by controller is '?' for an invalid command or ':' or space for a valid command */ -static int DMC2280Send(pDMC2280Driv self, char *command) { +static int DMC2280Send(pDMC2280Driv self, /*@unique@*/char *command) { char cmdValid, pError[ERRLEN], reply[256]; char *GetEMsg = "TC 1"; int status; @@ -193,6 +224,7 @@ static int DMC2280Send(pDMC2280Driv self, char *command) { return HWFault; snprintf(pError, ERRLEN, "DMC2280ERROR: Bad command '%s'", command); SCWrite(self->pCon, reply, eError); + SICSLogWrite(self->pCon, reply, eError); self->errorCode = BADCMD; return FAILURE; default: @@ -202,7 +234,9 @@ static int DMC2280Send(pDMC2280Driv self, char *command) { } } -static int DMC2280Receive(pDMC2280Driv self, char *reply) { +/** \brief + */ +static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) { int i, status, retries=20, dataLen=255; for (i=0; icontroller, reply, &dataLen); @@ -220,7 +254,15 @@ static int DMC2280Receive(pDMC2280Driv self, char *reply) { } return FAILURE; } -/*---------------------------------------------------------------------*/ +/** \brief Reads motor position, implements the GetPosition + * method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \param *fPos contains the motor position in physical units after a call. + * \return + * - OKOK request succeeded + * - HWFault request failed + * */ static int DMC2280GetPos(void *pData, float *fPos){ pDMC2280Driv self = NULL; char reply[1024]; @@ -249,7 +291,16 @@ static int DMC2280GetPos(void *pData, float *fPos){ } return OKOK; } -/*----------------------------------------------------------------------*/ +/** \brief DMC2280 implementation of the RunTo + * method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \param fValue target position in physical units, software zeros + * have already been applied. + * \return + * - OKOK request succeeded + * - HWFault request failed + * */ static int DMC2280Run(void *pData,float fValue){ pDMC2280Driv self = NULL; char axis; @@ -290,20 +341,30 @@ static int DMC2280Run(void *pData,float fValue){ } -/*------------------------------------------------------------------------*/ +/** \brief Returns the motor status while it's moving, + * implements the GetStatus method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \return + * - HWFault hardware fault or status cannot be read. + * - HWPosFault controller was unable to position the motor. + * - HWBusy The motor is still driving. + * - HWWarn There is a warning from the controller. + * - HWIdle The motor has finished driving and is idle. + */ static int DMC2280Status(void *pData){ pDMC2280Driv self = NULL; char cmd[CMDLEN]; int switches; char switchesAscii[10], reply[256]; - int moving, fwd_limit_active, rvrs_limit_active, errorlimit; + bool moving, fwd_limit_active, rvrs_limit_active, errorlimit; int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus; int SHOULD_FIXPOS=1, should_fixpos; self = (pDMC2280Driv)pData; assert(self != NULL); /* Get status of switches - * see TS (Tell Switches) in Galil manc2xx */ + * see TS (Tell Switches) in Galil manc2xx.pdf */ snprintf(cmd, CMDLEN, "TS%c", self->axisLabel); if (FAILURE == DMC2280Send(self, cmd)) return HWFault; @@ -311,8 +372,8 @@ static int DMC2280Status(void *pData){ return HWFault; sscanf(switchesAscii, "%d", &switches); moving = (switches & STATUSMOVING)>0; - fwd_limit_active = !(switches & STATUSFWDLIMIT)>0; - rvrs_limit_active = !(switches & STATUSRVRSLIMIT)>0; + fwd_limit_active = !((switches & STATUSFWDLIMIT)>0); + rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0); errorlimit = (switches & STATUSERRORLIMIT)>0; if (fwd_limit_active && rvrs_limit_active) { @@ -364,7 +425,14 @@ static int DMC2280Status(void *pData){ return HWIdle; } } -/*----------------------------------------------------------------------*/ +/** \brief DMC2280 implementation of the GetError + * method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \param *iCode error code returned to abstract motor + * \param *error error message + * \param errLen maximum error message length allowed by abstract motor + */ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ pDMC2280Driv self = NULL; self = (pDMC2280Driv)pData; @@ -415,14 +483,24 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){ strncpy(error,"Axis error exceeds error limit",(size_t)errLen); break; case IMPOSSIBLE_LIM_SW: - strncpy(error,"Both limit switches seem active, maybe the polarity is set 'active low'. You should controller the controller with CN 1,-1,-1,0", (size_t)errLen); + strncpy(error,"Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen); break; default: /* FIXME What's the default */ break; } } -/*----------------------------------------------------------------------*/ + +/** \brief Attempts to recover from an error. Implements the TryAndFixIt + * method in the MotorDriver interface. + * + * \param *pData provides access to a motor's data + * \param iCode error code returned by DMC2280Error + * \param fValue unused, target position + * \return A return code which informs the abstract motors next action. + * - MOTREDO try to redo the last move. + * - MOTFAIL move failed, give up. + */ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ pDMC2280Driv self = NULL; @@ -445,9 +523,14 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){ } return MOTFAIL; } -/*----------------------------------------------------------------------*/ -/* Emergency Halt - * Uses maximum deceleration */ +/** \brief Emergency halt. Implements the Halt + * method in the MotorDriver interface. + * + * Uses maximum deceleration + * \param *pData provides access to a motor's data + * + * XXX Does abstract motor use the return values? + */ static int DMC2280Halt(void *pData){ pDMC2280Driv self = NULL; char cmd[CMDLEN]; @@ -469,7 +552,18 @@ static int DMC2280Halt(void *pData){ return 1; } -/*--------------------------------------------------------------------*/ +/** \brief Fetches the value of the named parameter, + * implements the GetDriverPar method in the MotorDriver interface. + * + * Note: The GetDriverPar method in the MotorDriver interface only + * allows float values to be returned. + * \param *pData (r) provides access to a motor's data + * \param *name (r) the name of the parameter to fetch. + * \param *fValue (w) the parameter's value. + * \return + * - 1 request succeeded + * - 0 request failed + * */ static int DMC2280GetPar(void *pData, char *name, float *fValue){ pDMC2280Driv self = NULL; @@ -514,7 +608,20 @@ static int DMC2280GetPar(void *pData, char *name, } return 0; } -/*--------------------------------------------------------------------*/ + +/** \brief Sets the named parameter, implements the SetDriverPar + * method in the MotorDriver interface. + * + * Note: The SetDriverPar method in the MotorDriver interface only + * allows float values to be set. + * \param *pData (rw) provides access to a motor's data + * \param *pCon (r) connection object. + * \param *name (r) of the parameter to set. + * \param *newValue (r) new value. + * \return + * - 1 request succeeded + * - 0 request failed + * */ static int DMC2280SetPar(void *pData, SConnection *pCon, char *name, float newValue){ pDMC2280Driv self = NULL; @@ -641,7 +748,8 @@ static void KillDMC2280(void *pData){ /*@-temptrans@*/ free(self); /*@+temptrans@*/ return; } -static prs232 DMC2280Connect(SConnection *pCon, char *buffer, int port) { +/*@only@*/ prs232 createRS232(char *host, int iPort); +/*@null@ @only@*/ static prs232 DMC2280Connect(SConnection *pCon, char *buffer, int port) { prs232 controller=NULL; char pError[ERRLEN]; int usecTimeout = 50000; /* 50msec timeout */ @@ -659,6 +767,7 @@ static prs232 DMC2280Connect(SConnection *pCon, char *buffer, int port) { "ERROR: failed to connect to %s at port %d", controller->pHost, controller->iPort); SCWrite(pCon,pError,eError); + KillRS232(controller); return NULL; } setRS232ReplyTerminator(controller,"&\r\n:"); @@ -668,8 +777,9 @@ static prs232 DMC2280Connect(SConnection *pCon, char *buffer, int port) { } /* Get configuration parameter */ -static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *parName, int mustHave ) { - char *pPtr=NULL, pError[ERRLEN]; +/*@observer@*/static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *parName, int mustHave ) { + char *pPtr=NULL; + char pError[ERRLEN]; pPtr = Tcl_GetVar2(pTcl,params,parName,TCL_GLOBAL_ONLY); if((mustHave == _REQUIRED) && !pPtr){ snprintf(pError, ERRLEN,"ERROR: No '%s' parameter given for dmc2280 motor", parName); @@ -696,15 +806,17 @@ static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *p allocate and initialize data structure */ pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv)); - if(!pNew){ + if(NULL == pNew){ (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver", eError); return NULL; } /* Get hostname and port from the list of named parameters */ - if ((pPtr=getParam(pCon, interp, params,"port",1)) == NULL) return NULL; + if ((pPtr=getParam(pCon, interp, params,"port",1)) == NULL) + goto FailedCreateDMC2280; sscanf(pPtr,"%d",&port); - if ((pPtr=getParam(pCon, interp, params,"host",1)) == NULL) return NULL; + if ((pPtr=getParam(pCon, interp, params,"host",1)) == NULL) + goto FailedCreateDMC2280; strncpy(buffer,pPtr, 131); /* Connect to the controller */ @@ -713,7 +825,7 @@ static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *p if( pNew->controller == NULL ) { snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); SCWrite(pCon,pError,eError); - return NULL; + goto FailedCreateDMC2280; } /*FIXME Tell splint that there's no memory leak because pointers are being initialised here */ @@ -741,25 +853,25 @@ static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *p pNew->ListDriverPar = DMC2280List; pNew->KillPrivate = KillDMC2280; if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) - return NULL; + goto FailedCreateDMC2280; sscanf(pPtr,"%s",pNew->units); if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) - return NULL; + goto FailedCreateDMC2280; sscanf(pPtr,"%f",&(pNew->speed)); pNew->maxSpeed = pNew->speed; if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) - return NULL; + goto FailedCreateDMC2280; sscanf(pPtr,"%f",&(pNew->accel)); pNew->maxAccel = pNew->accel; if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) - return NULL; + goto FailedCreateDMC2280; sscanf(pPtr,"%f",&(pNew->decel)); pNew->maxDecel = pNew->decel; if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) - return NULL; + goto FailedCreateDMC2280; sscanf(pPtr,"%c",&(pNew->axisLabel)); if ((pPtr=getParam(pCon, interp, params,"stepsPerX",_REQUIRED)) == NULL) - return NULL; + goto FailedCreateDMC2280; sscanf(pPtr,"%d",&(pNew->stepsPerX)); if ((pPtr=getParam(pCon, interp, params,"motorHome",_OPTIONAL)) == NULL) pNew->motorHome=0; @@ -794,6 +906,10 @@ static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *p exit(EXIT_FAILURE); /* TODO Initialise current position and target to get a sensible initial list output */ return (MotorDriver *)pNew; +FailedCreateDMC2280: + if (NULL != pNew) + free(pNew); + return NULL; }