linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
a3467cb4ef7c60122332cc2cbf22acc17a89b831
sicspsi/ecbdriv.c
koennecke 88fa7449af - Renamed tasdrive to ptasdrive in order to help debugging 
- Added ritastorage in order to solve storage order problem for
  RITA's area detector
2006-09-13 07:12:57 +00:00

1303 lines
34 KiB
C
Raw Blame History

/*------------------------------------------------------------------------
this is a motor driver for the Risoe motor controllers within the
ECB system. The motor is controlled through functions invoked in the
Z80 processor of the ECB system which is connected through a GPIB
bus to the wider world. This driver has to do a lot of extra things:
- it has to convert from physical values to motor steps.
- Quite a few parameters, such as ramping parameters,
have to be downloaded to the ECB
- Risoe motors may have a virtual encoder or a real encoder.
- The motor may have to control air cushions as well.
- Tricky backlash handling. Backlash handling ensures that a position is
always arrived at from a defined direction. If backlash is applied
a restart flag is set in ECBRunTo. ECBGetStatus checks for that and
causes the motor to drive back to the position actually desired.
This driver support only P2048a motor controllers, as these are the
only ones which seem to have arrived at PSI. The P1648 and Tridynamic
things are not supported.
Multiplexed motors: Originally the ECB supported 24 motors. This did
prove to be not enough. Therefore another device called P2234e was
introduced which allowed to run 8 motors from one controller port. In this
case the motor parameters have to be copied to the ECB before
driving the motor. Multiplexing is selected through the parameter MULT.
MULT 0 means no multiplexing, MULT > 0 makes MULT the number of the
motor in the multiplexer. MULT is now also used to flag a download of
parameters to the ECB. In such a case MULT is -1.
Some of this code was taken from the tascom driver for the ECB.
copyright: see file COPYRIGHT
Mark Koennecke, January 2003
--------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <errno.h>
#include <tcl.h>
#include <math.h>
#include <unistd.h>
#include <fortify.h>
#include <sics.h>
#include <motor.h>
#include <obpar.h>
#include <splitter.h>
#include "ecb.h"
/*------------------------------------------------------------------------
Parameter indexes in ObPar array and meanings
-------------------------------------------------------------------------*/
#define ENCODER 0 /* encoder number, 0 if no encoder */
#define CONTROL 1 /* control signals, > 1 means required. */
#define RANGE 2 /* 0 = slow, 1 = fast */
#define MULT 3 /* 0 = not multiplexed, > 0 multiplex motor number*/
#define MULTCHAN 16 /* multiplexer channel */
#define ACCTIME 4 /* acceleration time: 500, 1000 or 2000 milSecs */
#define ROTDIR 5 /* rotation direction */
#define STARTSPEED 6 /* start speed: 100-500 steps/s */
#define MAXSPEED 7 /* maximum speed: 100-2000 steps/sec */
#define SLOWAUTO 8 /* slow speed in auto mode */
#define SLOWMAN 9 /* slow speed in manual mode */
#define DELAY 10 /* start delay 0 - 2500 millSecs */
#define OFFSET 11 /* encoder offset */
#define TOLERANCE 12 /* tolerance in steps */
#define STEPS2DEG 13 /* conversion factor motor steps to Degree */
#define DEG2STEP 14 /* conversion factor from degree to encoder digits */
#define BACKLASH 15 /* motor backlash */
#define PORT 17 /* ECB port when multiplexed */
#define DEC 18 /* Decimals in display */
#define MAXPAR 20 /* 1 extra for the sentinel, do not forget to initialize! */
/*------------------------------ ECB defines -------------------------*/
#define MAX_ENCODER 40
#define FENCOR 167 /* read encoder */
#define MOREAD 145 /* read motor steps */
#define MOPARA 140 /* motor parameter */
#define MOCLOA 146
#define ABS(x) (x < 0 ? -(x) : (x))
#define MOSTEP 141
#define MOSTAT 144
/********************* error codes *************************************/
#define COMMERROR -300
#define ECBMANUELL -301
#define ECBINUSE -302
#define UNIDENTIFIED -303
#define ECBINHIBIT -304
#define ECBRUNNING -305
#define ECBSTART -306
#define ECBLIMIT -307
#define ECBREADERROR -308
/*================== The Driver data structure ============================*/
typedef struct __ECBMotorDriv {
/* general motor driver interface
fields. REQUIRED!
*/
float fUpper; /* upper limit */
float fLower; /* lower limit */
char *name;
int (*GetPosition)(void *self,float *fPos);
int (*RunTo)(void *self, float fNewVal);
int (*GetStatus)(void *self);
void (*GetError)(void *self, int *iCode,
char *buffer, int iBufLen);
int (*TryAndFixIt)(void *self,int iError,
float fNew);
int (*Halt)(void *self);
int (*GetDriverPar)(void *self, char *name,
float *value);
int (*SetDriverPar)(void *self,SConnection *pCon,
char *name, float newValue);
void (*ListDriverPar)(void *self, char *motorName,
SConnection *pCon);
void (*KillPrivate)(void *self);
/* ECB specific fields */
pECB ecb; /* ECB controller for everything */
int ecbIndex; /* motor index in ECB */
int errorCode;
int restart; /* flag if we have to restart
because of backlash
compensation
*/
float restartTarget; /* target to restart to */
ObPar driverPar[MAXPAR]; /* parameters */
} ECBMOTDriv, *pECBMotDriv;
/*=======================================================================
Reading the motor position means reading the encoder if such a thing
is present or the counted motor steps (Pseudo Encoder) if not.
If the ECB answers us, the value has to be converted to physical
values.
----------------------------------------------------------------------*/
static int readEncoder(pECBMotDriv self, long *digits){
int status;
Z80_reg in, out;
Ecb_pack data;
in.c = (unsigned char)ObVal(self->driverPar,ENCODER) + MAX_ENCODER;
status = ecbExecute(self->ecb,FENCOR,in,&out);
if(!status){
self->errorCode = COMMERROR;
return status;
}
/* pack bytes */
data.b.byt3 = 0;
data.b.byt2 = out.b;
data.b.byt1 = out.d;
data.b.byt0 = out.e;
if(out.c != 1){
*digits = -data.result;
} else {
*digits = data.result;
}
return OKOK;
}
/*---------------------------------------------------------------------*/
static int readPseudoEncoder(pECBMotDriv self, long *digits){
int status;
Z80_reg in, out;
Ecb_pack data;
in.c = (unsigned char)self->ecbIndex;
status = ecbExecute(self->ecb,MOREAD,in,&out);
if(!status){
self->errorCode = COMMERROR;
return status;
}
/* pack bytes */
data.b.byt3 = 0;
data.b.byt2 = out.b;
data.b.byt1 = out.d;
data.b.byt0 = out.e;
if(out.c != 1){
*digits = -data.result;
} else {
*digits = data.result;
}
return OKOK;
}
/*----------------------------------------------------------------------*/
int ECBMOTGetPos(void *pData, float *fPos){
pECBMotDriv self = (pECBMotDriv)pData;
long digits = 0;
int status;
double step2degree;
assert(self);
self->errorCode = 0;
if((int)ObVal(self->driverPar,ENCODER) > 0){
status = readEncoder(self, &digits);
step2degree = ObVal(self->driverPar,DEG2STEP);
if(step2degree == 0.0){
step2degree = 1;
}
*fPos = (digits/step2degree) -
ObVal(self->driverPar,OFFSET);
return status;
} else {
status = readPseudoEncoder(self, &digits);
}
step2degree = ObVal(self->driverPar,STEPS2DEG);
if(step2degree == 0.0){
step2degree = 1.;
}
*fPos = (float)( (double)digits/step2degree);
return status;
}
/*========================================================================
In order to start a motor we need to do a couple of steps:
- check if the motors parameters have been changed or it is a multiplexed
motor. In each case download the motor parameters.
- the direction of the motor has to be determined, the speed to be
selected etc.
- Then the motor can be started.
------------------------------------------------------------------------*/
static int mustDownload(pECBMotDriv self){
int multi;
multi = (int)rint(ObVal(self->driverPar,MULT));
if(multi > 0 || multi < 0) {
return 1;
} else {
return 0;
}
}
/*--------------------------------------------------------------------*/
static int checkMotorResult(pECBMotDriv self, Z80_reg out){
/*
checks the return values from a motor function invocation
and sets error codes in case of problems.
*/
if(out.c == '\0'){
switch(out.b){
case 128:
self->errorCode = ECBMANUELL;
break;
case 64:
self->errorCode = ECBINHIBIT;
break;
case 32:
case 96:
self->errorCode = ECBRUNNING;
break;
case 1:
self->errorCode = ECBSTART;
break;
case 16:
self->errorCode = ECBLIMIT;
break;
case 4:
self->errorCode = ECBINUSE;
break;
default:
fprintf(stderr,"Unidentified ECB motor error code %d\n", out.b);
self->errorCode = UNIDENTIFIED;
break;
}
return 0;
} else {
return 1;
}
}
/*---------------------------------------------------------------------*/
static int loadAcceleration(pECBMotDriv self){
unsigned char parameter;
Z80_reg in, out;
int accel, status;
accel = (int)rint(ObVal(self->driverPar,ACCTIME));
if(accel == 500){
parameter = 1;
}else if(accel == 1000){
parameter = 2;
}else if(accel == 2000){
parameter = 3;
} else {
parameter = 0;
}
/*
apply rotation direction mask
*/
if(ObVal(self->driverPar,ROTDIR) < 0){
parameter += 128;
}
in.c = (unsigned char)self->ecbIndex;
in.b = 7;
in.e = parameter;
in.d = 0;
out.d = out.e = out.b = out.c = 0;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(!status){
self->errorCode = COMMERROR;
return 0;
}
if(!checkMotorResult(self, out)){
return 0;
}
return 1;
}
/*--------------------------- speed tables ------------------------------*/
#define SPEED_TAB3 64 /* Size of speed table */
const unsigned int low_2048[SPEED_TAB3] = {
1, 2, 3, 4, 5, 6, 8, 10, 12, 14,
16, 20, 24, 28, 32, 36, 40, 44, 48, 56,
64, 72, 80, 88, 96,104,112,120,128,136,
144,152,160,168,176,184,192,200,208,216,
224,236,248,260,272,284,296,308,320,332,
344,356,368,380,392,404,416,428,440,452,
464,476,488,500 };
#define SPEED_TAB4 96 /* Size of speed table */
const unsigned int high_2048[SPEED_TAB4] = {
11, 15, 20, 27, 36, 47, 59, 74,
93, 107, 124, 143, 165, 190, 213, 239,
268, 298, 331, 368, 405, 446, 491, 536,
585, 632, 683, 731, 783, 827, 873, 922,
974, 1028, 1085, 1146, 1211, 1278, 1349, 1424,
1503, 1587, 1675, 1720, 1820, 1913, 2014, 2123,
2237, 2360, 2483, 2620, 2755, 2905, 3058, 3221,
3384, 3575, 3756, 3945, 4150, 4370, 4600, 4800,
5000, 5250, 5533, 5822, 6120, 6440, 6770, 7090,
7450, 7800, 8130, 8500, 8900, 9320, 9730, 10200,
10700, 11200, 11700, 12200, 12800, 13300, 13900, 14500,
15100, 15800, 16700, 17300, 18000, 18600, 19300, 20000 };
/*---------------------------------------------------------------------*/
static unsigned char getSpeedIndex(float value,
int range, int *actualValue ){
unsigned char index;
const unsigned int *table;
int length;
if(range == 0){
table = low_2048;
length = SPEED_TAB3;
} else {
table = high_2048;
length = SPEED_TAB4;
}
for(index = 0; index < length-1; index++){
if(table[index] >= value){
break;
}
}
*actualValue = table[index];
return index;
}
/*--------------------------------------------------------------------*/
static int loadSpeed(pECBMotDriv self, float value, int code){
unsigned char parameter;
Z80_reg in, out;
int accel, status, actual;
parameter = getSpeedIndex(value, (int)rint(ObVal(self->driverPar,RANGE)),
&actual);
in.c = (unsigned char)self->ecbIndex;
in.b = code;
in.e = parameter;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(!status){
self->errorCode = COMMERROR;
return 0;
}
if(!checkMotorResult(self, out)){
return 0;
}
return 1;
}
/*-------------------------------------------------------------------*/
static int loadDelay(pECBMotDriv self){
int parameter;
Z80_reg in, out;
int accel, status;
unsigned char control;
parameter = (int)rint(ObVal(self->driverPar,DELAY));
control = (unsigned char)rint(ObVal(self->driverPar,CONTROL));
if(control & 3){
parameter = 5;
} else{
parameter/= 10;
}
in.c = (unsigned char)self->ecbIndex;
in.b = 8;
in.e = parameter;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(!status){
self->errorCode = COMMERROR;
return 0;
}
if(!checkMotorResult(self, out)){
return 0;
}
return 1;
}
/*---------------------------------------------------------------------*/
static int loadMulti(pECBMotDriv self){
int multi, mult_chan;
Z80_reg in, out;
int status;
multi = rint(ObVal(self->driverPar,MULT));
if(multi <= 0){
return 1; /* not multiplexed */
}
mult_chan = (unsigned char)rint(ObVal(self->driverPar,MULTCHAN));
in.b = -1; /* SET_PORT */
in.d = (unsigned char)(multi + (mult_chan << 4));
in.e = (unsigned char)rint(ObVal(self->driverPar,PORT));
in.c = self->ecbIndex;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
return 1;
}
/*------------------------------------------------------------------*/
static int loadOffset(pECBMotDriv self, float offset){
Z80_reg in, out;
int status;
Ecb_pack data;
/*
ignored
*/
if(ObVal(self->driverPar,ENCODER) <=.0){
return 1;
}
data.result = offset * ObVal(self->driverPar,STEPS2DEG);
in.b = data.b.byt2;
in.d = data.b.byt1;
in.e = data.b.byt0;
in.c = (unsigned char)rint(ObVal(self->driverPar,ENCODER));
status = ecbExecute(self->ecb,168,in,&out);
if(status == 1){
self->driverPar[OFFSET].fVal = offset;
} else {
self->errorCode = COMMERROR;
}
return status;
}
/*---------------------------------------------------------------------
This loads the gearing parameters for the CRT display. This should
not have any influence on the running of the motor
------------------------------------------------------------------------*/
static double To_ten(int v) {
double vv;
vv = 1.0;
if (v == 1)
vv = 10.0;
if (v == 2)
vv = 100.0;
if (v == 3)
vv = 1000.0;
if (v == 4)
vv = 10000.0;
if (v == 5)
vv = 100000.0;
if (v == 6)
vv = 1000000.0;
if (v == 7)
vv = 10000000.0;
return (vv);
}
/*----------------------------------------------------------------------*/
static int loadGearing(pECBMotDriv self){
int status;
double dgear;
int gdec, dec = 0, ratio;
Ecb_pack data;
Z80_reg in, out;
dec = (int)ObVal(self->driverPar,DEC);
in.c = self->ecbIndex;
dgear = (double) ObVal(self->driverPar,STEPS2DEG);;
/* Calculate decimals in display and gearing ratio for the ECB system*/
gdec = (int) (1.0 + (log10(dgear - .01)));
if (dec < gdec)
dec = gdec; /* Display does not work with decimals < gdec */
ratio = (long) (0.5 + dgear*To_ten(6 + 1 - dec));
data.result = ratio;
in.b = data.b.byt2;
in.d = data.b.byt1;
in.e = data.b.byt0;
status = ecbExecute(self->ecb,174,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
}
if(ObVal(self->driverPar,ENCODER) == 0){
in.b = self->ecbIndex;
} else {
in.b = 1;
in.e = (unsigned char)ObVal(self->driverPar,ENCODER);
}
in.d = 0;
in.e = dec;
status = ecbExecute(self->ecb,173,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
}
return 1;
}
/*----------------------------------------------------------------------*/
static int downloadECBParam(pECBMotDriv self){
int status, parameter;
unsigned char func_code;
Z80_reg in, out;
/*
We assume that all parameters have useful values. It is the task of
SetDriverPar to ensure just that!
*/
if((status = loadAcceleration(self) <= 0)){
return 0;
}
if((status = loadSpeed(self,ObVal(self->driverPar,STARTSPEED),6)) <= 0){
return 0;
}
if((status = loadSpeed(self,ObVal(self->driverPar,MAXSPEED),5)) <= 0){
return 0;
}
if((status = loadSpeed(self,ObVal(self->driverPar,SLOWAUTO),4)) <= 0){
return 0;
}
if((status = loadSpeed(self,ObVal(self->driverPar,SLOWMAN),10)) <= 0){
return 0;
}
if((status = loadDelay(self)) <= 0){
return 0;
}
if((status = loadMulti(self)) <= 0){
return 0;
}
if((status = ecbLoadEncoder(self->ecb)) <= 0){
return 0;
}
if((status = loadOffset(self,ObVal(self->driverPar,OFFSET))) <= 0){
return 0;
}
/*
It would be good practice to read the parameters written back
in order to check them. This does not seem to be supported with the
ECB system though.
*/
if(ObVal(self->driverPar,MULT) < 0.){
self->driverPar[MULT].fVal = .0;
}
if((status = loadGearing(self)) <= 0){
return 0;
}
return 1;
}
/*--------------------------------------------------------------------*/
int degree2Step(pECBMotDriv self, float degree)
{
double steps;
steps = degree*ObVal(self->driverPar,STEPS2DEG);
if (ObVal(self->driverPar,ENCODER) > .0)
steps = steps*ObVal(self->driverPar,DEG2STEP);
if(degree < 0){
steps = - steps;
}
return ((int) steps);
}
/*----------------------------------------------------------------------
controlMotor enables or disables the motor, according to flag enable.
This is also used to switch on air cushions and the like.
------------------------------------------------------------------------*/
static int controlMotor(pECBMotDriv self, int enable){
int status, delay, control;
Z80_reg in, out;
/*
nothing to do if we are not in control
*/
control = (int)rint(ObVal(self->driverPar,CONTROL));
if(!(control & 1)){
return 1;
}
delay = (int)rint(ObVal(self->driverPar,DELAY));
if(enable == 1){
/*
enabling
*/
in.e = 12; /* 8 + 4 */
in.b = 11; /* set control signal */
in.c = (unsigned char)self->ecbIndex;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
/*
wait for air cushions to settle
*/
usleep(delay);
return 1;
}else {
/*
disable air cushions
*/
in.e = 8;
in.b = 11; /* set control signal */
in.c = self->ecbIndex;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
usleep(delay);
/*
clear enable
*/
in.e = 0;
in.b = 11; /* set control signal */
in.c = self->ecbIndex;
status = ecbExecute(self->ecb,MOPARA,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
usleep(delay);
return 1;
}
}
/*-----------------------------------------------------------------------*/
static int ECBRunTo(void *pData, float newPosition){
pECBMotDriv self = (pECBMotDriv)pData;
long digits = 0;
int status;
float oldValue, diff, steps2degree, backlash;
Ecb_pack data;
Z80_reg in, out;
assert(self);
if(mustDownload(self)){
status = downloadECBParam(self);
if(!status){
return 0;
}
}
/*
read old position
*/
status = ECBMOTGetPos(self,&oldValue);
if(status != 1){
return status;
}
/*
do not start if there
*/
diff = newPosition - oldValue;
steps2degree= ObVal(self->driverPar,STEPS2DEG);
if(ABS(diff) <= .5/steps2degree + ObVal(self->driverPar,TOLERANCE)){
return OKOK;
}
/*
save restartTarget for backlash handling
*/
self->restartTarget = newPosition;
/*
enable and push up airy cushions
*/
status = controlMotor(self,1);
if(status != 1){
return status;
}
/*
write control data
*/
in.d = 0;
if(diff > .0){
in.d |= 32; /* positive direction */
}
in.d |= 16; /* interrupts */
if(rint(ObVal(self->driverPar,RANGE)) == 1.){
in.d |= 64; /* fast speed */
}
in.c = (unsigned char)self->ecbIndex;
status = ecbExecute(self->ecb,MOCLOA,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
/*
calculate steps
*/
self->restart = 0;
backlash = ObVal(self->driverPar,BACKLASH);
if(diff < 0){
diff = -diff;
if(backlash > 0.){
diff += backlash;
self->restart = 1;
}
} else {
if(backlash < 0.){
diff -= backlash;
self->restart = 1;
}
}
data.result = degree2Step(self,diff);
/*
finally start the motor
*/
in.b = data.b.byt2;
in.d = data.b.byt1;
in.e = data.b.byt0;
in.c = (unsigned char)self->ecbIndex;
status = ecbExecute(self->ecb,MOSTEP,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
if(!checkMotorResult(self, out)){
return 0;
}
return OKOK;
}
/*=======================================================================*/
static int checkStatusResponse(pECBMotDriv self, Z80_reg out){
if(out.c == '\0'){
if(out.b & 4) {
self->errorCode = ECBINUSE;
} else {
self->errorCode = ECBREADERROR;
}
return HWFault;
}
if(out.b & 128){
self->errorCode = ECBMANUELL;
return HWFault;
} else if(out.b & 32){
return HWBusy;
} else if(out.b & 16){
self->errorCode = ECBLIMIT;
return HWFault;
}
return HWIdle;
}
/*----------------------------------------------------------------------*/
static int ECBGetStatus(void *pData){
pECBMotDriv self = (pECBMotDriv)pData;
Z80_reg in, out;
int status, result;
assert(self);
in.c = (unsigned char)self->ecbIndex;
in.b = 12;
status = ecbExecute(self->ecb,MOSTAT,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return HWFault;
}
result = checkStatusResponse(self,out);
if(result == HWFault || result == HWIdle){
/*
run down airy cushions ........
*/
controlMotor(self,0);
}
/*
take care of backlash.....
*/
if(result == HWIdle && self->restart == 1){
self->restart = 0;
ECBRunTo(self,self->restartTarget);
return HWBusy;
}
return result;
}
/*======================================================================*/
static void ECBGetError(void *pData, int *iCode, char *buffer, int bufferlen){
pECBMotDriv self = (pECBMotDriv)pData;
char pBueffel[132];
assert(self);
*iCode = self->errorCode;
switch(self->errorCode){
case COMMERROR:
strncpy(buffer,"communication problem with ECB",bufferlen);
break;
case ECBMANUELL:
strncpy(buffer,"ECB is in manual mode, trying to switch...",bufferlen);
break;
case ECBINUSE:
strncpy(buffer,"Power supply is in use",bufferlen);
break;
case ECBINHIBIT:
strncpy(buffer,"motor is inhibited",bufferlen);
break;
case ECBRUNNING:
strncpy(buffer,"motor is running",bufferlen);
break;
case ECBSTART:
strncpy(buffer,"failed to start motor",bufferlen);
break;
case ECBLIMIT:
strncpy(buffer,"hit limit switch or amplifier error",bufferlen);
break;
default:
strncpy(buffer,"unidentified error code",bufferlen);
break;
}
}
/*========================================================================*/
static int ECBHalt(void *pData){
pECBMotDriv self = (pECBMotDriv)pData;
Z80_reg in, out;
unsigned char par = 2;
assert(self);
if(rint(ObVal(self->driverPar,RANGE)) == 1){
par |= 64;
}
in.b = 9;
in.e = par;
in.c = (unsigned char)self->ecbIndex;
ecbExecute(self->ecb,MOPARA,in,&out);
self->restart = 0;
return 1;
}
/*=======================================================================*/
static int ECBTryAndFixIt(void *pData, int iCode, float fNew){
pECBMotDriv self = (pECBMotDriv)pData;
int result;
Z80_reg in, out;
int i;
assert(self);
switch(iCode){
case ECBMANUELL:
in.d = 1 ;
ecbExecute(self->ecb,162,in,&out);
result = MOTREDO;
break;
case ECBRUNNING:
ECBHalt(pData);
self->restart = 0;
for(i = 0; i < 7; i++){
if(ECBGetStatus(pData) == HWIdle){
break;
}
SicsWait(1);
}
result = MOTREDO;
break;
case COMMERROR:
ecbClear(self->ecb);
result = MOTREDO;
break;
default:
result = MOTFAIL;
break;
}
return result;
}
/*=======================================================================*/
static int ECBGetDriverPar(void *pData,char *name, float *value){
pECBMotDriv self = (pECBMotDriv)pData;
ObPar *par = NULL;
assert(self);
if(strcmp(name,"ecbindex") == 0){
*value = self->ecbIndex;
return 1;
}
/*
* catch the command parameters
*/
if(strcmp(name,"putpos") == 0 || strcmp(name,"download") == 0){
*value = .0;
return 1;
}
par = ObParFind(self->driverPar,name);
if(par != NULL){
*value = par->fVal;
return 1;
} else {
return 0;
}
}
/*=====================================================================*/
static float fixAccTime(float newValue){
float corrected, min, diff;
int val, possibleValues[4] = { 500, 1000, 2000, 5000}, i;
val = (int)rint(newValue);
min = 9999999.99;
for(i = 0; i < 4; i++){
diff = val - possibleValues[i];
if(ABS(diff) < min){
min = ABS(diff);
corrected = possibleValues[i];
}
}
return corrected;
}
/*--------------------------------------------------------------------*/
static void setDownloadFlag(pECBMotDriv self, int parNumber){
int mustDownload;
switch(parNumber){
case CONTROL:
case MULT:
case MULTCHAN:
case ACCTIME:
case ROTDIR:
case STARTSPEED:
case MAXSPEED:
case SLOWAUTO:
case SLOWMAN:
case DELAY:
mustDownload = 1;
break;
default:
mustDownload = 0;
break;
}
if(mustDownload && (self->driverPar[MULT].fVal == 0)){
self->driverPar[MULT].fVal = -1.0;
}
}
/*--------------------------------------------------------------------*/
static int putMotorPosition(pECBMotDriv self, float newValue){
Z80_reg in,out;
Ecb_pack data;
float oldPos;
int status;
if(ABS(ObVal(self->driverPar,ENCODER)) > .1){
status = ECBMOTGetPos(self,&oldPos);
if(status != 1){
return status;
}
return loadOffset(self,oldPos - newValue);
} else {
data.result = newValue*ObVal(self->driverPar,STEPS2DEG);
in.b = data.b.byt2;
in.d = data.b.byt1;
in.e = data.b.byt0;
in.c = (unsigned char)self->ecbIndex;
status = ecbExecute(self->ecb,142,in,&out);
if(status != 1){
self->errorCode = COMMERROR;
return 0;
}
if(!checkMotorResult(self, out)){
return 0;
}
}
return 1;
}
/*---------------------------------------------------------------------*/
static int ECBSetDriverPar(void *pData, SConnection *pCon, char *name,
float newValue){
pECBMotDriv self = (pECBMotDriv)pData;
int parNumber, speedNumber, actualSpeed, status;
char pBueffel[256];
float correctedValue;
assert(self);
/*
only managers shall edit these parameters....
*/
if(!SCMatchRights(pCon,usMugger)){
return 0;
}
/*
this is rather a command and forces a parameter download
to the ECB
*/
if(strcmp(name,"download") == 0){
status = downloadECBParam(self);
if(status != 1){
ECBGetError(self,&actualSpeed, pBueffel,254);
SCWrite(pCon,pBueffel,eError);
return status;
}
}
/*
this is another command and assigns a position to the current
motor place
*/
if(strcmp(name,"putpos") == 0){
status = putMotorPosition(self,newValue);
if(status != 1){
ECBGetError(self,&actualSpeed, pBueffel,254);
SCWrite(pCon,pBueffel,eError);
return status;
}
return 1;
}
/*
get the parameter number
*/
parNumber = ObParIndex(self->driverPar,name);
if(parNumber < 0){
return 0;
}
/*
make these parameters right, at least as far as we can .......
*/
switch(parNumber){
case ACCTIME:
correctedValue = fixAccTime(newValue);
break;
case STARTSPEED:
getSpeedIndex(rint(newValue),1,&actualSpeed);
correctedValue = actualSpeed;
if(correctedValue < 10){
correctedValue = 10;
}
if(correctedValue > 4400){
correctedValue = 4400;
}
break;
case MAXSPEED:
getSpeedIndex(rint(newValue),1,&actualSpeed);
correctedValue = actualSpeed;
break;
case SLOWAUTO:
case SLOWMAN:
getSpeedIndex(rint(newValue),0,&actualSpeed);
correctedValue = actualSpeed;
if(correctedValue > 500){
correctedValue = 500;
}
break;
case DELAY:
correctedValue = newValue;
if(correctedValue > 2500){
correctedValue = 2500;
}
break;
case RANGE:
correctedValue = newValue;
if(correctedValue != 0.0 && correctedValue != 1.0){
correctedValue = .0; /* slow by default! */
}
break;
case ENCODER:
if(newValue < 0. || newValue > 3.){
SCWrite(pCon,"ERROR: encoder numbers can only be 0 - 3", eError);
return 0;
} else if(newValue == 0){
correctedValue = newValue;
} else {
ecbAssignEncoder(self->ecb,(int)newValue, self->ecbIndex);
correctedValue = newValue;
}
break;
case STEPS2DEG:
case DEG2STEP:
if(ABS(newValue) < .1){
correctedValue = 1.;
} else {
correctedValue = newValue;
}
break;
case OFFSET:
correctedValue = newValue;
break;
default:
correctedValue = newValue;
break;
}
if(ABS(correctedValue - newValue) > 0.){
sprintf(pBueffel,"WARNING: Illegal value %6.2f verbosely coerced to %6.2f",
newValue,correctedValue);
SCWrite(pCon,pBueffel,eWarning);
}
ObParSet(self->driverPar,self->name,name,correctedValue,pCon);
setDownloadFlag(self,parNumber);
return 1;
}
/*=========================================================================*/
static void ECBListPar(void *pData, char *motorName, SConnection *pCon){
pECBMotDriv self = (pECBMotDriv)pData;
char pBueffel[256];
int i;
assert(self);
for(i = 0; i < MAXPAR-1; i++){
snprintf(pBueffel,255,"%s.%s = %f",
motorName,self->driverPar[i].name,
self->driverPar[i].fVal);
SCWrite(pCon,pBueffel,eValue);
}
}
/*========================================================================*/
static int interpretArguments(pECBMotDriv self, SConnection *pCon,
int argc, char *argv[]){
char pBueffel[256];
TokenList *pList, *pCurrent;
pList = SplitArguments(argc,argv);
if(!pList || argc < 4){
SCWrite(pCon,"ERROR: no arguments to CreateECBMotor",eError);
return 0;
}
pCurrent = pList;
/*
first should be the name of the ECB to use
*/
if(pCurrent->Type != eText){
sprintf(pBueffel,"ERROR: expected EDB name, got: %s",
pCurrent->text);
DeleteTokenList(pList);
return 0;
}
self->ecb = (pECB)FindCommandData(pServ->pSics,pCurrent->text,"ECB");
if(!self->ecb){
sprintf(pBueffel,"ERROR: %s is no ECB controller",pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return 0;
}
/*
next the motor number
*/
pCurrent = pCurrent->pNext;
if(pCurrent->Type != eInt){
sprintf(pBueffel,"ERROR: expected int motor number, got %s",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return 0;
}
self->ecbIndex = pCurrent->iVal;
/*
next the limits
*/
pCurrent = pCurrent->pNext;
if(pCurrent->Type != eFloat){
sprintf(pBueffel,"ERROR: expected float type limit, got %s",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return 0;
}
self->fLower = pCurrent->fVal;
pCurrent = pCurrent->pNext;
if(pCurrent->Type != eFloat){
sprintf(pBueffel,"ERROR: expected float type limit, got %s",
pCurrent->text);
SCWrite(pCon,pBueffel,eError);
DeleteTokenList(pList);
return 0;
}
self->fUpper = pCurrent->fVal;
DeleteTokenList(pList);
return 1;
}
/*-----------------------------------------------------------------------*/
static void initializeParameters(pECBMotDriv self){
ObParInit(self->driverPar,ENCODER,"encoder",0,usMugger);
ObParInit(self->driverPar,CONTROL,"control",0,usMugger);
ObParInit(self->driverPar,RANGE,"range",1,usMugger);
ObParInit(self->driverPar,MULT,"multi",0,usMugger);
ObParInit(self->driverPar,MULTCHAN,"multchan",0,usMugger);
ObParInit(self->driverPar,ACCTIME,"acceleration",500,usMugger);
ObParInit(self->driverPar,ROTDIR,"rotation_dir",1,usMugger);
ObParInit(self->driverPar,STARTSPEED,"startspeed",100,usMugger);
ObParInit(self->driverPar,MAXSPEED,"maxspeed",2000,usMugger);
ObParInit(self->driverPar,SLOWAUTO,"auto",100,usMugger);
ObParInit(self->driverPar,SLOWMAN,"manuell",100,usMugger);
ObParInit(self->driverPar,DELAY,"delay",50,usMugger);
ObParInit(self->driverPar,OFFSET,"offset",0,usMugger);
ObParInit(self->driverPar,TOLERANCE,"dtolerance",0.,usMugger);
ObParInit(self->driverPar,STEPS2DEG,"step2deg",1,usMugger);
ObParInit(self->driverPar,DEG2STEP,"step2dig",0,usMugger);
ObParInit(self->driverPar,BACKLASH,"backlash",0,usMugger);
ObParInit(self->driverPar,PORT,"port",0,usMugger);
ObParInit(self->driverPar,DEC,"dec",1,usMugger);
ObParInit(self->driverPar,MAXPAR-1,"tueet",-100,-100); /* sentinel! */
}
/*=======================================================================*/
void KillECBMotor(void *pDriver){
int i;
pECBMotDriv self = (pECBMotDriv)pDriver;
for(i = 0; i < MAXPAR; i++){
if(self->driverPar[i].name != NULL){
free(self->driverPar[i].name);
}
}
}
/*------------------------------------------------------------------------*/
MotorDriver *CreateECBMotor(SConnection *pCon, int argc, char *argv[]){
pECBMotDriv self = NULL;
self = (pECBMotDriv)malloc(sizeof(ECBMOTDriv));
if(self == NULL){
return NULL;
}
memset(self,0,sizeof(ECBMOTDriv));
if(!interpretArguments(self,pCon,argc,argv)){
free(self);
return 0;
}
initializeParameters(self);
/*
set function pointers
*/
self->GetPosition = ECBMOTGetPos;
self->RunTo = ECBRunTo;
self->GetStatus = ECBGetStatus;
self->GetError = ECBGetError;
self->TryAndFixIt = ECBTryAndFixIt;
self->Halt = ECBHalt;
self->GetDriverPar = ECBGetDriverPar;
self->SetDriverPar = ECBSetDriverPar;
self->ListDriverPar = ECBListPar;
self->KillPrivate = KillECBMotor;
self->errorCode = 0;
return (MotorDriver *)self;
}
Reference in New Issue View Git Blame Copy Permalink