Adding doxygen documentation and splint tags.
r1005 | ffr | 2006-05-26 16:58:46 +1000 (Fri, 26 May 2006) | 2 lines
This commit is contained in:
Ferdi Franceschini
committed by
Douglas Clowes
Douglas Clowes
parent
11710db517
commit
ae91e9ab5b
@@ -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 <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
#include <assert.h>
|
||||
@@ -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; i<retries; i++) {
|
||||
status=readRS232(self->controller, 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; i<retries; i++) {
|
||||
status=readRS232TillTerm(self->controller, 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user