- GPIB/ECB modules completed, ecb motor driver in progress
This commit is contained in:
cvs
695
ecbdriv.c
Normal file
695
ecbdriv.c
Normal file
@@ -0,0 +1,695 @@
|
||||
/*------------------------------------------------------------------------
|
||||
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.
|
||||
|
||||
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 "fortify.h"
|
||||
#include "sics.h"
|
||||
#include "motor.h"
|
||||
#include "obpar.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 MAXPAR 18 /* 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
|
||||
|
||||
/********************* t-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
|
||||
/*================== 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);
|
||||
|
||||
|
||||
/* ECB specific fields */
|
||||
pECB ecb; /* ECB controller for everything */
|
||||
int ecbIndex; /* motor index in ECB */
|
||||
long targetInSteps;
|
||||
int errorCode;
|
||||
ObPar driverPar[MAXPAR];
|
||||
} 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.b;
|
||||
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.b;
|
||||
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);
|
||||
} 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)nintf(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:
|
||||
self->errorCode = ECBRUNNING;
|
||||
break;
|
||||
case 1:
|
||||
self->errorCode = ECBSTART;
|
||||
break;
|
||||
case 16:
|
||||
self->errorCode = ECBLIMIT;
|
||||
break;
|
||||
case 4:
|
||||
self->errorCode = ECBINUSE;
|
||||
break;
|
||||
default:
|
||||
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)nintf(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;
|
||||
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(pECBMotDriv self, int value){
|
||||
unsigned char index;
|
||||
const unsigned int *table;
|
||||
int range, length;
|
||||
|
||||
range = (int)nintf(ObVal(self->driverPar,RANGE));
|
||||
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;
|
||||
}
|
||||
}
|
||||
return index;
|
||||
}
|
||||
/*--------------------------------------------------------------------*/
|
||||
static int loadSpeed(pECBMotDriv self, float value, int code){
|
||||
unsigned char parameter;
|
||||
Z80_reg in, out;
|
||||
int accel, status;
|
||||
|
||||
parameter = getSpeedIndex(self,value);
|
||||
|
||||
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)nintf(ObVal(self->driverPar,DELAY));
|
||||
control = (unsigned char)nintf(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){
|
||||
unsigned char multi, mult_chan;
|
||||
Z80_reg in, out;
|
||||
int status;
|
||||
|
||||
multi = nintf(ObVal(self->driverPar,MULT));
|
||||
if(multi <= 0){
|
||||
return 1; /* not multiplexed */
|
||||
}
|
||||
|
||||
mult_chan = (unsigned char)nintf(ObVal(self->driverPar,MULTCHAN));
|
||||
in.b = -1; /* SET_PORT */
|
||||
in.d = (unsigned char)(multi + (mult_chan << 4));
|
||||
in.e = self->ecbIndex;
|
||||
status = ecbExecute(self->ecb,MOPARA,in,&out);
|
||||
if(status != 1){
|
||||
self->errorCode = COMMERROR;
|
||||
return 0;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
write control data
|
||||
*/
|
||||
in.d = 0;
|
||||
if(diff > .0){
|
||||
in.d |= 32; /* positive direction */
|
||||
}
|
||||
in.d |= 16; /* interrupts */
|
||||
if(nintf(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
|
||||
*/
|
||||
backlash = ObVal(self->driverPar,BACKLASH);
|
||||
if(diff < 0){
|
||||
diff = -diff;
|
||||
if(backlash > 0){
|
||||
diff += backlash;
|
||||
}
|
||||
} else {
|
||||
if(backlash < 0){
|
||||
diff -= backlash;
|
||||
}
|
||||
}
|
||||
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 ECBGetStatus(void *pData){
|
||||
pECBMotDriv self = (pECBMotDriv)pData;
|
||||
Z80_reg in, out;
|
||||
int status;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
status = checkMotorResult(self,out);
|
||||
if(status == 0){
|
||||
if(self->errorCode == ECBRUNNING){
|
||||
self->errorCode = 0;
|
||||
return HWBusy;
|
||||
} else {
|
||||
return HWFault;
|
||||
}
|
||||
} else {
|
||||
return HWIdle;
|
||||
}
|
||||
}
|
||||
/*======================================================================*/
|
||||
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",bufferlen);
|
||||
break;
|
||||
default:
|
||||
strncpy(buffer,"unidentified error code",bufferlen);
|
||||
break;
|
||||
}
|
||||
}
|
||||
/*=======================================================================*/
|
||||
static int ECBTryAndFixIt(void *pData, int iCode, float fNew){
|
||||
pECBMotDriv self = (pECBMotDriv)pData;
|
||||
int result;
|
||||
Z80_reg in, out;
|
||||
|
||||
assert(self);
|
||||
|
||||
switch(iCode){
|
||||
case ECBMANUELL:
|
||||
in.d =1 ;
|
||||
ecbExecute(self->ecb,162,in,&out);
|
||||
result = MOTREDO;
|
||||
break;
|
||||
case COMMERROR:
|
||||
ecbClear(self->ecb);
|
||||
result = MOTREDO;
|
||||
break;
|
||||
default:
|
||||
result = MOTFAIL;
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
/*========================================================================*/
|
||||
static int ECBHalt(void *pData){
|
||||
pECBMotDriv self = (pECBMotDriv)pData;
|
||||
Z80_reg in, out;
|
||||
unsigned char par = 2;
|
||||
|
||||
assert(self);
|
||||
|
||||
if(nintf(ObVal(self->driverPar,RANGE)) == 1){
|
||||
par |= 64;
|
||||
}
|
||||
|
||||
in.b = 9;
|
||||
in.e = par;
|
||||
ecbExecute(self->ecb,MOPARA,in,&out);
|
||||
return 1;
|
||||
}
|
||||
/*=======================================================================*/
|
||||
static int ECBGetDriverPar(void *pData,char *name, float *value){
|
||||
pECBMotDriv self = (pECBMotDriv)pData;
|
||||
ObPar *par = NULL;
|
||||
|
||||
assert(self);
|
||||
|
||||
par = ObParFind(self->driverPar,name);
|
||||
if(par != NULL){
|
||||
*value = par->fVal;
|
||||
return 1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
/*=====================================================================*/
|
||||
static int ECBSetDriverPar(void *pData, SConnection *pCon, char *name,
|
||||
float newValue){
|
||||
pECBMotDriv self = (pECBMotDriv)pData;
|
||||
int parNumber;
|
||||
char pBueffel[256];
|
||||
float correctedValue;
|
||||
|
||||
parNumber = ObParIndex(self->driverPar,name);
|
||||
if(parNumber < 0){
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
only managers shall edit these parameters....
|
||||
*/
|
||||
if(!SCMatchRights(pCon,usMugger)){
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
assert(self);
|
||||
return 1;
|
||||
}
|
||||
Reference in New Issue
Block a user