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

Adding doxygen documentation and splint tags.

Browse Source 
r1005 | ffr | 2006-05-26 16:58:46 +1000 (Fri, 26 May 2006) | 2 lines
This commit is contained in:
Ferdi Franceschini
2006-05-26 16:58:46 +10:00
committed by Douglas Clowes
parent 11710db517
commit ae91e9ab5b
1 changed files with 171 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

226
site_ansto/motor_dmc2280.c
View File

@@ -1,13 +1,14 @@
/*------------------------------------------------------------------------ /** \file motor_dmc2280.c
Implements a SICS motor object with a MotorDriver interface. * \brief Driver for Galil DMC2280 motor controller.
The control and communications functions are implemented in a separate layer *
in instrument/dmc2280.tcl * Implements a SICS motor object with a MotorDriver interface.
*
Copyright: see file Copyright.txt * Copyright: see file Copyright.txt
*
Ferdi Franceschini November 2005 * Ferdi Franceschini November 2005
-----------------------------------------------------------------------*/ */
#include <stdlib.h> #include <stdlib.h>
#include <stdbool.h>
#include <math.h> #include <math.h>
#include <float.h> #include <float.h>
#include <assert.h> #include <assert.h>
@@ -25,6 +26,13 @@ Ferdi Franceschini November 2005
#include "splint/splint_SCinter.h" #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 The motor driver structure. Please note that the first set of fields has
be identical with the fields of AbstractModriv in ../modriv.h be identical with the fields of AbstractModriv in ../modriv.h
@@ -59,21 +67,21 @@ typedef struct __MoDriv {
int oredMsr; int oredMsr;
int lastValue; int lastValue;
int iConfig; int iConfig;
char units[256]; /* physical units for axis */ char units[256]; /**< physical units for axis */
float speed; /* physical units per second */ float speed; /**< physical units per second */
float maxSpeed; /* physical units per second */ float maxSpeed; /**< physical units per second */
float accel; /* physical units per second^2 */ float accel; /**< physical units per second^2 */
float maxAccel; /* physical units per second^2 */ float maxAccel; /**< physical units per second^2 */
float decel; /* physical units per second^2 */ float decel; /**< physical units per second^2 */
float maxDecel; /* physical units per second^2 */ float maxDecel; /**< physical units per second^2 */
char axisLabel; char axisLabel;
char lastCmd[1024]; char lastCmd[1024];
float home; /* home position for axis, default=0 */ float home; /**< home position for axis, default=0 */
int motorHome; /* motor home position in steps */ int motorHome; /**< motor home position in steps */
int stepsPerX; /* steps per physical unit */ int stepsPerX; /**< steps per physical unit */
int abs_endcoder; /* Flag = 1 if there is an abs enc */ int abs_endcoder; /**< Flag = 1 if there is an abs enc */
int absEncHome; /* Home position in counts for abs enc */ int absEncHome; /**< Home position in counts for abs enc */
int cntsPerX; /* absolute encoder counts per physical unit */ int cntsPerX; /**< absolute encoder counts per physical unit */
} DMC2280Driv, *pDMC2280Driv; } DMC2280Driv, *pDMC2280Driv;
/*------------------- error codes ----------------------------------*/ /*------------------- error codes ----------------------------------*/
#define BADADR -1 // Unknown host/port? #define BADADR -1 // Unknown host/port?
@@ -142,7 +150,17 @@ static int motDecel(pDMC2280Driv self, float decel) {
return motDecel; 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; int i, status, retries=20, dataLen=1;
for (i=0; i<retries; i++) { for (i=0; i<retries; i++) {
status=readRS232(self->controller, reply, &dataLen); status=readRS232(self->controller, reply, &dataLen);
@@ -159,11 +177,24 @@ static int DMC2280ReadChar(pDMC2280Driv self, char *reply) {
} }
return FAILURE; 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 '?' for an invalid command or
':' or space for a valid command */ ':' 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 cmdValid, pError[ERRLEN], reply[256];
char *GetEMsg = "TC 1"; char *GetEMsg = "TC 1";
int status; int status;
@@ -193,6 +224,7 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
return HWFault; return HWFault;
snprintf(pError, ERRLEN, "DMC2280ERROR: Bad command '%s'", command); snprintf(pError, ERRLEN, "DMC2280ERROR: Bad command '%s'", command);
SCWrite(self->pCon, reply, eError); SCWrite(self->pCon, reply, eError);
SICSLogWrite(self->pCon, reply, eError);
self->errorCode = BADCMD; self->errorCode = BADCMD;
return FAILURE; return FAILURE;
default: 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; int i, status, retries=20, dataLen=255;
for (i=0; i<retries; i++) { for (i=0; i<retries; i++) {
status=readRS232TillTerm(self->controller, reply, &dataLen); status=readRS232TillTerm(self->controller, reply, &dataLen);
@@ -220,7 +254,15 @@ static int DMC2280Receive(pDMC2280Driv self, char *reply) {
} }
return FAILURE; 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){ static int DMC2280GetPos(void *pData, float *fPos){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
char reply[1024]; char reply[1024];
@@ -249,7 +291,16 @@ static int DMC2280GetPos(void *pData, float *fPos){
} }
return OKOK; 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){ static int DMC2280Run(void *pData,float fValue){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
char axis; 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){ static int DMC2280Status(void *pData){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
char cmd[CMDLEN]; char cmd[CMDLEN];
int switches; int switches;
char switchesAscii[10], reply[256]; 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 SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
int SHOULD_FIXPOS=1, should_fixpos; int SHOULD_FIXPOS=1, should_fixpos;
self = (pDMC2280Driv)pData; self = (pDMC2280Driv)pData;
assert(self != NULL); assert(self != NULL);
/* Get status of switches /* 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); snprintf(cmd, CMDLEN, "TS%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd)) if (FAILURE == DMC2280Send(self, cmd))
return HWFault; return HWFault;
@@ -311,8 +372,8 @@ static int DMC2280Status(void *pData){
return HWFault; return HWFault;
sscanf(switchesAscii, "%d", &switches); sscanf(switchesAscii, "%d", &switches);
moving = (switches & STATUSMOVING)>0; moving = (switches & STATUSMOVING)>0;
fwd_limit_active = !(switches & STATUSFWDLIMIT)>0; fwd_limit_active = !((switches & STATUSFWDLIMIT)>0);
rvrs_limit_active = !(switches & STATUSRVRSLIMIT)>0; rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0);
errorlimit = (switches & STATUSERRORLIMIT)>0; errorlimit = (switches & STATUSERRORLIMIT)>0;
if (fwd_limit_active && rvrs_limit_active) { if (fwd_limit_active && rvrs_limit_active) {
@@ -364,7 +425,14 @@ static int DMC2280Status(void *pData){
return HWIdle; 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){ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
self = (pDMC2280Driv)pData; 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); strncpy(error,"Axis error exceeds error limit",(size_t)errLen);
break; break;
case IMPOSSIBLE_LIM_SW: 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; break;
default: default:
/* FIXME What's the default */ /* FIXME What's the default */
break; 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){ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
@@ -445,9 +523,14 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
} }
return MOTFAIL; return MOTFAIL;
} }
/*----------------------------------------------------------------------*/ /** \brief Emergency halt. Implements the Halt
/* Emergency Halt * method in the MotorDriver interface.
* Uses maximum deceleration */ *
* 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){ static int DMC2280Halt(void *pData){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
char cmd[CMDLEN]; char cmd[CMDLEN];
@@ -469,7 +552,18 @@ static int DMC2280Halt(void *pData){
return 1; 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, static int DMC2280GetPar(void *pData, char *name,
float *fValue){ float *fValue){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
@@ -514,7 +608,20 @@ static int DMC2280GetPar(void *pData, char *name,
} }
return 0; 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, static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue){ char *name, float newValue){
pDMC2280Driv self = NULL; pDMC2280Driv self = NULL;
@@ -641,7 +748,8 @@ static void KillDMC2280(void *pData){
/*@-temptrans@*/ free(self); /*@+temptrans@*/ /*@-temptrans@*/ free(self); /*@+temptrans@*/
return; 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; prs232 controller=NULL;
char pError[ERRLEN]; char pError[ERRLEN];
int usecTimeout = 50000; /* 50msec timeout */ 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", "ERROR: failed to connect to %s at port %d",
controller->pHost, controller->iPort); controller->pHost, controller->iPort);
SCWrite(pCon,pError,eError); SCWrite(pCon,pError,eError);
KillRS232(controller);
return NULL; return NULL;
} }
setRS232ReplyTerminator(controller,"&\r\n:"); setRS232ReplyTerminator(controller,"&\r\n:");
@@ -668,8 +777,9 @@ static prs232 DMC2280Connect(SConnection *pCon, char *buffer, int port) {
} }
/* Get configuration parameter */ /* Get configuration parameter */
static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *parName, int mustHave ) { /*@observer@*/static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *parName, int mustHave ) {
char *pPtr=NULL, pError[ERRLEN]; char *pPtr=NULL;
char pError[ERRLEN];
pPtr = Tcl_GetVar2(pTcl,params,parName,TCL_GLOBAL_ONLY); pPtr = Tcl_GetVar2(pTcl,params,parName,TCL_GLOBAL_ONLY);
if((mustHave == _REQUIRED) && !pPtr){ if((mustHave == _REQUIRED) && !pPtr){
snprintf(pError, ERRLEN,"ERROR: No '%s' parameter given for dmc2280 motor", parName); 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 allocate and initialize data structure
*/ */
pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv)); pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv));
if(!pNew){ if(NULL == pNew){
(void) SCWrite(pCon,"ERROR: no memory to allocate motor driver", (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
eError); eError);
return NULL; return NULL;
} }
/* Get hostname and port from the list of named parameters */ /* 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); 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); strncpy(buffer,pPtr, 131);
/* Connect to the controller */ /* Connect to the controller */
@@ -713,7 +825,7 @@ static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *p
if( pNew->controller == NULL ) { if( pNew->controller == NULL ) {
snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor); snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
SCWrite(pCon,pError,eError); SCWrite(pCon,pError,eError);
return NULL; goto FailedCreateDMC2280;
} }
/*FIXME Tell splint that there's no memory leak because pointers are being initialised here */ /*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->ListDriverPar = DMC2280List;
pNew->KillPrivate = KillDMC2280; pNew->KillPrivate = KillDMC2280;
if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL)
return NULL; goto FailedCreateDMC2280;
sscanf(pPtr,"%s",pNew->units); sscanf(pPtr,"%s",pNew->units);
if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL)
return NULL; goto FailedCreateDMC2280;
sscanf(pPtr,"%f",&(pNew->speed)); sscanf(pPtr,"%f",&(pNew->speed));
pNew->maxSpeed = pNew->speed; pNew->maxSpeed = pNew->speed;
if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL)
return NULL; goto FailedCreateDMC2280;
sscanf(pPtr,"%f",&(pNew->accel)); sscanf(pPtr,"%f",&(pNew->accel));
pNew->maxAccel = pNew->accel; pNew->maxAccel = pNew->accel;
if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL)
return NULL; goto FailedCreateDMC2280;
sscanf(pPtr,"%f",&(pNew->decel)); sscanf(pPtr,"%f",&(pNew->decel));
pNew->maxDecel = pNew->decel; pNew->maxDecel = pNew->decel;
if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL)
return NULL; goto FailedCreateDMC2280;
sscanf(pPtr,"%c",&(pNew->axisLabel)); sscanf(pPtr,"%c",&(pNew->axisLabel));
if ((pPtr=getParam(pCon, interp, params,"stepsPerX",_REQUIRED)) == NULL) if ((pPtr=getParam(pCon, interp, params,"stepsPerX",_REQUIRED)) == NULL)
return NULL; goto FailedCreateDMC2280;
sscanf(pPtr,"%d",&(pNew->stepsPerX)); sscanf(pPtr,"%d",&(pNew->stepsPerX));
if ((pPtr=getParam(pCon, interp, params,"motorHome",_OPTIONAL)) == NULL) if ((pPtr=getParam(pCon, interp, params,"motorHome",_OPTIONAL)) == NULL)
pNew->motorHome=0; pNew->motorHome=0;
@@ -794,6 +906,10 @@ static char *getParam(SConnection *pCon, Tcl_Interp *pTcl, char *params, char *p
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
/* TODO Initialise current position and target to get a sensible initial list output */ /* TODO Initialise current position and target to get a sensible initial list output */
return (MotorDriver *)pNew; return (MotorDriver *)pNew;
FailedCreateDMC2280:
if (NULL != pNew)
free(pNew);
return NULL;
} }