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

Modified during MK's visit.

Browse Source 
r1462 | ffr | 2007-02-12 12:06:19 +1100 (Mon, 12 Feb 2007) | 2 lines
This commit is contained in:
Ferdi Franceschini
2007-02-12 12:06:19 +11:00
committed by Douglas Clowes
parent 9ac6c7b414
commit 634f2023b1
3 changed files with 158 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
site_ansto/Makefile
View File

@ -16,8 +16,7 @@ default: all
EXTRA=nintf.o
PSI_CLEAN_MATRIX = rm -f ../*.o; $(MAKE) -C ../matrix $(MFLAGS) clean
PSI_CFLAGS = -I./ -I$(HDFROOT)/include -DHDF5 -DNXXML\
-fwritable-strings -DCYGNUS -DNONINTF -g $(DFORTIFY)\
-I /usr/local/include/json/
-fwritable-strings -DCYGNUS -DNONINTF -g $(DFORTIFY)
PSI_SLIBS = matrix/libmatrix.a
PSI_LIBS = -L$(HDFROOT)/lib $(NILIB)\
-ltcl8.4 $(HDFROOT)/lib/libhdf5.a \
@ -76,8 +75,12 @@ OBJ= site_ansto.o anstoutil.o\
anstohttp.o \
hmcontrol_ansto.o
all: ../matrix/libmatrix.a $(COREOBJ:%=../%) libansto.a libhardsup
$(CC) -g -o SICServer $(COREOBJ:%=../%) $(SUBLIBS) $(PSI_SLIBS:%=../%) $(PSI_LIBS) $(GHTTP_LIBS)
all: ../matrix/libmatrix.a $(COREOBJ:%=../%) $(EXTRA:%=../%) libansto.a libhardsup
$(CC) -g -o SICServer $(COREOBJ:%=../%) $(EXTRA:%=../%) $(SUBLIBS) $(PSI_SLIBS:%=../%) $(PSI_LIBS) $(GHTTP_LIBS)
#TODO Add targets for other instruments
echidna: all
make -C instrument/hrpd
libansto.a: $(OBJ)
rm -f libansto.a

3
site_ansto/make_gen_variables
View File

@ -25,7 +25,8 @@ SOBJ = network.o ifile.o conman.o SCinter.o splitter.o passwd.o \
hmdata.o nxscript.o tclintimpl.o sicsdata.o mcstascounter.o \
mcstashm.o initializer.o remob.o tclmotdriv.o protocol.o \
sinfox.o sicslist.o cone.o hipadaba.o sicshipadaba.o statistics.o \
$(EXTRA)
moregress.o hdbcommand.o multicounter.o regresscter.o histregress.o \
sicshdbadapter.o polldriv.o sicspoll.o statemon.o
MOTOROBJ = motor.o simdriv.o
COUNTEROBJ = countdriv.o simcter.o counter.o

185
site_ansto/motor_dmc2280.c
View File

@ -8,9 +8,8 @@
* Ferdi Franceschini November 2005
*
* TODO
* - check for ESTOP flag on controller.
* - ESTOP=1 means emergency stop, motion on all axes must stop.
* - ESTOP=0 means all OK.
* - check for motors enabled on plc
* - Check error bit, see Dan's email
*/
#include <stdlib.h>
/* ISO C Standard: 7.16 Boolean type and values <stdbool.h> */
@ -26,6 +25,7 @@
#include <modriv.h>
#include <motor.h>
#include <dynstring.h>
#include <time.h>
#include "anstoutil.h"
/*
@ -61,9 +61,9 @@ typedef struct __MoDriv {
/* general motor driver interface
fields. _REQUIRED!
*/
float fUpper; /* upper limit */
float fLower; /* lower limit */
char *name;
float fUpper; /**< hard upper limit */
float fLower; /**< hard lower limit */
char *name; /**< motor name */
int (*GetPosition)(void *self, float *fPos);
int (*RunTo)(void *self, float fNewVal);
int (*GetStatus)(void *self);
@ -102,6 +102,9 @@ typedef struct __MoDriv {
int absEncHome; /**< Home position in counts for abs enc */
int cntsPerX; /**< absolute encoder counts per physical unit */
int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */
float lastPosition; /**< Position at last position check */
struct timeval time_lastPos_set; /**< Time when lastPosition was set */
float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
} DMC2280Driv, *pDMC2280Driv;
/*------------------- error codes ----------------------------------*/
#define BADADR -1 // NOT SET: Unknown host/port?
@ -118,6 +121,7 @@ typedef struct __MoDriv {
#define ERRORLIM -13
#define IMPOSSIBLE_LIM_SW -14
#define BGFAIL -15 // NOT SET
#define BLOCKED -16
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errorss exceed limit */
@ -142,7 +146,10 @@ typedef struct __MoDriv {
#define MAXACCEL "maxaccel"
#define DECEL "decel"
#define MAXDECEL "maxdecel"
#define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"
static int DMC2280Halt(void *pData);
static void set_lastPos(void *pData, float posn);
static int DMC2280SetPar(void *pData, SConnection *pCon,
char *name, float newValue);
static int DMC2280Receive(pDMC2280Driv self, /*@out@*/ char *reply);
@ -194,7 +201,7 @@ static int motDecel(pDMC2280Driv self, float axisDecel) {
* \see SUCCESS FAILURE
*/
static int DMC2280ReadChar(pDMC2280Driv self, /*@out@*/char *reply) {
int i, status, retries=20, dataLen=1;
int i, status, retries=1, dataLen=1;
reply[0] = '\0';
for (i=0; i<retries; i++) {
status=readRS232(self->controller, reply, &dataLen);
@ -287,7 +294,7 @@ static int DMC2280Send(pDMC2280Driv self, char *command) {
* \see SUCCESS FAILURE
*/
static int DMC2280Receive(pDMC2280Driv self, /*@out@*/char *reply) {
int i, status, retries=20, dataLen=255;
int i, status, retries=1, dataLen=255;
reply[0] = '\0';
for (i=0; i<retries; i++) {
status=readRS232TillTerm(self->controller, reply, &dataLen);
@ -370,6 +377,26 @@ static int getDMCSetting(void *pData, enum dmcsetting cmdIndex){
}
}
/** \brief Reads absolute encoder.
*
* \param *pos is the absolute encoder reading on SUCCESS.
* \return
* SUCCESS
* FAILURE
*/
static int readAbsEnc(pDMC2280Driv self, float *pos) {
char reply[1024];
char cmd[CMDLEN];
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return FAILURE;
if (FAILURE == DMC2280Receive(self, reply))
return FAILURE;
*pos = (float) atoi(reply);
return SUCCESS;
}
/** \brief Reads motor position, implements the GetPosition
* method in the MotorDriver interface.
@ -390,12 +417,8 @@ static int DMC2280GetPos(void *pData, float *fPos){
self = (pDMC2280Driv)pData;
assert(self != NULL);
if (1 == self->abs_endcoder) {
snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
if (readAbsEnc(self, &absEncPos) == FAILURE)
return HWFault;
if (FAILURE == DMC2280Receive(self, reply))
return HWFault;
absEncPos =(float)atof(reply);
*fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
} else {
snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
@ -423,10 +446,11 @@ static int DMC2280Run(void *pData,float fValue){
char axis;
char cmd[CMDLEN], SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
int absEncHome, stepsPerX, motorHome, cntsPerX, newAbsPosn;
float target;
float target, currPos;
self = (pDMC2280Driv)pData;
assert(self != NULL);
DMC2280GetPos(pData, &currPos);
set_lastPos(pData, currPos);
axis=self->axisLabel;
motorHome = self->motorHome;
stepsPerX=self->stepsPerX;
@ -459,6 +483,53 @@ static int DMC2280Run(void *pData,float fValue){
return OKOK;
}
/** \brief Record the given posn and timestamp it.
*
* \param *pData provides access to a motor's data
* \param posn, the axis position which you want to remember.
* */
static void set_lastPos(void *pData, float posn) {
pDMC2280Driv self;
self = (pDMC2280Driv)pData;
assert(self != NULL);
self->lastPosition = posn;
gettimeofday(&(self->time_lastPos_set), NULL);
}
/** \brief Check if the axis position has changed significantly since
* the last check.
*
* The position change is checked against the 'precision' at intervals of
* pDMC2280Driv->blockage_ckInterval.
*
* \param *pData provides access to a motor's data
* \return
* - 1 MOTOR OK, position has changed significantly during move
* - 0 MOTOR BLOCKED, no significant change in position detected.
*/
static int checkPosition(void *pData) {
float precision, currPos;
long int usec_TimeDiff;
struct timeval now;
pDMC2280Driv self;
self = (pDMC2280Driv)pData;
assert(self != NULL);
gettimeofday(&now, NULL);
usec_TimeDiff = (now.tv_sec - (self->time_lastPos_set).tv_sec)*1e6 + (now.tv_usec - (self->time_lastPos_set).tv_usec);
if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
return 1;
if (self->pMot == NULL)
self->pMot = FindMotor(pServ->pSics, self->name);
MotorGetPar(self->pMot,"precision",&precision);
DMC2280GetPos(pData, &currPos);
if ( precision - fabs(self->lastPosition - currPos) >= FLT_EPSILON) {
return 0;
} else {
set_lastPos(pData, currPos);
return 1;
}
}
/** \brief Returns the motor status while it's moving,
* implements the GetStatus method in the MotorDriver interface.
@ -505,8 +576,16 @@ static int DMC2280Status(void *pData){
return HWFault;
}
if (moving) {
/* If pos hasn't changed since last
* check then stop and scream */
if (checkPosition(pData) == 0) {
DMC2280Halt(pData);
self->errorCode = BLOCKED;
return HWFault;
} else {
self->errorCode = BADBSY;
return HWBusy;
}
} else {
/* If motor stopped check limits and error status */
if (fwd_limit_active) {
@ -632,6 +711,9 @@ static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
case IMPOSSIBLE_LIM_SW:
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;
case BLOCKED:
strncpy(error,"STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
break;
default:
/* FIXME What's the default */
break;
@ -661,6 +743,7 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
case BADCMD:
//case TIMEOUT:
case BADPAR:
case BLOCKED:
return MOTFAIL;
case POSFAULT:
case BADSEND:
@ -671,8 +754,11 @@ static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
case NOTCONNECTED:
initRS232(self->controller);
return MOTREDO;
}
break;
default:
return MOTFAIL;
break;
}
}
/** \brief Emergency halt. Implements the Halt
* method in the MotorDriver interface.
@ -692,6 +778,9 @@ static int DMC2280Halt(void *pData){
assert(self != NULL);
/* Stop motor */
snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
if (FAILURE == DMC2280Send(self, cmd))
return HWFault;
else
@ -761,6 +850,18 @@ static int DMC2280GetPar(void *pData, char *name,
*fValue = self->maxDecel;
return 1;
}
if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
*fValue = self->blockage_ckInterval;
return 1;
}
if (self->abs_endcoder != 0) {
if (strcmp(name,"absenc") == 0) {
if (readAbsEnc(self, fValue) == SUCCESS)
return 1;
else
return 0;
}
}
return 0;
}
@ -793,12 +894,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
return 1;
else {
if ( (self->fLower - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", HOME, self->fLower);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
SCWrite(pCon, pError, eError);
return 1;
}
if ( (newValue - self->fUpper) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", HOME, self->fUpper);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
SCWrite(pCon, pError, eError);
return 1;
}
@ -816,20 +917,13 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
return 1;
}
/* Set hard limits, managers only */
if(strcmp(name,HARDLOWERLIM) == 0) {
/* Set interval between blocked motor checks,
* managers only */
if(strcmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->fLower = newValue;
return 1;
}
}
if(strcmp(name,HARDUPPERLIM) == 0) {
if(!SCMatchRights(pCon,usMugger))
return 1;
else {
self->fUpper = newValue;
self->blockage_ckInterval = newValue;
return 1;
}
}
@ -837,12 +931,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
/* Set speed */
if(strcmp(name,SPEED) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", SPEED, 0.0);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, SPEED, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", SPEED, self->maxSpeed);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, SPEED, self->maxSpeed);
SCWrite(pCon, pError, eError);
return 1;
}
@ -856,12 +950,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
/* Set Acceleration */
if(strcmp(name,ACCEL) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", ACCEL, 0.0);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxAccel ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", ACCEL, self->maxAccel);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel);
SCWrite(pCon, pError, eError);
return 1;
}
@ -875,12 +969,12 @@ static int DMC2280SetPar(void *pData, SConnection *pCon,
/* Set Deceleration */
if(strcmp(name,DECEL) == 0) {
if ((0.0 - newValue) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be greater than or equal to %f", DECEL, 0.0);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0);
SCWrite(pCon, pError, eError);
return 1;
}
if ((newValue - self->maxDecel ) > FLT_EPSILON) {
snprintf(pError, ERRLEN,"ERROR: %s must be less than or equal to %f", DECEL, self->maxDecel);
snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel);
SCWrite(pCon, pError, eError);
return 1;
}
@ -945,7 +1039,7 @@ static void KillDMC2280(/*@only@*/void *pData){
/*@null@*/ /*@only@*/ static prs232 DMC2280Connect(/*@dependent@*/SConnection *pCon, char *host, int port) {
prs232 controller=NULL;
char pError[ERRLEN];
int usecTimeout = 50000; /* 50msec timeout */
int msecTimeout = 5000;
controller=createRS232(host,port);
if (controller==NULL) {
@ -965,7 +1059,7 @@ static void KillDMC2280(/*@only@*/void *pData){
}
setRS232ReplyTerminator(controller,"&\r\n:");
setRS232SendTerminator(controller,"\r\n");
setRS232Timeout(controller, usecTimeout);
setRS232Timeout(controller, msecTimeout);
return controller;
}
@ -1067,8 +1161,19 @@ static void KillDMC2280(/*@only@*/void *pData){
pNew->SetDriverPar = DMC2280SetPar;
pNew->ListDriverPar = DMC2280List;
pNew->KillPrivate = KillDMC2280;
pNew->blockage_ckInterval = 0.5;
/* PARAMETERS: Fetch parameter values */
if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->fLower));
if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
}
sscanf(pPtr,"%f",&(pNew->fUpper));
if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
KillDMC2280(pNew);
return NULL;
@ -1133,15 +1238,15 @@ static void KillDMC2280(/*@only@*/void *pData){
/* Set speed */
snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed));
if (FAILURE == DMC2280Send(pNew, cmd))
exit(EXIT_FAILURE);
return NULL;
/* Set acceleration */
snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
if (FAILURE == DMC2280Send(pNew, cmd))
exit(EXIT_FAILURE);
return NULL;
/* Set deceleration */
snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
if (FAILURE == DMC2280Send(pNew, cmd))
exit(EXIT_FAILURE);
return NULL;
/* TODO Initialise current position and target to get a sensible initial list output */
return (MotorDriver *)pNew;
}