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

- ECB motors without encoders, with encoders and with control bits now work

Browse Source
This commit is contained in:
cvs
2003-01-24 15:38:32 +00:00
parent f45c3ebf55
commit e4d4643123
8 changed files with 649 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

564
ecbdriv.c
View File

@@ -1,5 +1,5 @@
/*------------------------------------------------------------------------
This is a motor driver for the Risoe motor controllers within the
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:
@@ -7,6 +7,11 @@
- 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
@@ -33,10 +38,13 @@
#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"
/*------------------------------------------------------------------------
@@ -81,6 +89,7 @@ Parameter indexes in ObPar array and meanings
#define ECBRUNNING -305
#define ECBSTART -306
#define ECBLIMIT -307
#define ECBREADERROR -308
/*================== The Driver data structure ============================*/
typedef struct __ECBMotorDriv {
/* general motor driver interface
@@ -108,9 +117,13 @@ Parameter indexes in ObPar array and meanings
/* ECB specific fields */
pECB ecb; /* ECB controller for everything */
int ecbIndex; /* motor index in ECB */
long targetInSteps;
int errorCode;
ObPar driverPar[MAXPAR];
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
@@ -133,7 +146,7 @@ static int readEncoder(pECBMotDriv self, long *digits){
/* pack bytes */
data.b.byt3 = 0;
data.b.byt2 = out.b;
data.b.byt1 = out.b;
data.b.byt1 = out.d;
data.b.byt0 = out.e;
if(out.c != 1){
*digits = -data.result;
@@ -158,7 +171,7 @@ static int readPseudoEncoder(pECBMotDriv self, long *digits){
/* pack bytes */
data.b.byt3 = 0;
data.b.byt2 = out.b;
data.b.byt1 = out.b;
data.b.byt1 = out.d;
data.b.byt0 = out.e;
if(out.c != 1){
*digits = -data.result;
@@ -179,6 +192,9 @@ int ECBMOTGetPos(void *pData, float *fPos){
if((int)ObVal(self->driverPar,ENCODER) > 0){
status = readEncoder(self, &digits);
*fPos = digits/ObVal(self->driverPar,DEG2STEP) -
ObVal(self->driverPar,OFFSET);
return status;
} else {
status = readPseudoEncoder(self, &digits);
}
@@ -201,7 +217,7 @@ In order to start a motor we need to do a couple of steps:
static int mustDownload(pECBMotDriv self){
int multi;
multi = (int)nintf(ObVal(self->driverPar,MULT));
multi = (int)rint(ObVal(self->driverPar,MULT));
if(multi > 0 || multi < 0) {
return 1;
} else {
@@ -249,7 +265,7 @@ static int loadAcceleration(pECBMotDriv self){
Z80_reg in, out;
int accel, status;
accel = (int)nintf(ObVal(self->driverPar,ACCTIME));
accel = (int)rint(ObVal(self->driverPar,ACCTIME));
if(accel == 500){
parameter = 1;
}else if(accel == 1000){
@@ -268,6 +284,8 @@ static int loadAcceleration(pECBMotDriv self){
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;
@@ -306,12 +324,12 @@ const unsigned int high_2048[SPEED_TAB4] = {
10700, 11200, 11700, 12200, 12800, 13300, 13900, 14500,
15100, 15800, 16700, 17300, 18000, 18600, 19300, 20000 };
/*---------------------------------------------------------------------*/
static unsigned char getSpeedIndex(pECBMotDriv self, int value){
static unsigned char getSpeedIndex(float value,
int range, int *actualValue ){
unsigned char index;
const unsigned int *table;
int range, length;
int length;
range = (int)nintf(ObVal(self->driverPar,RANGE));
if(range == 0){
table = low_2048;
length = SPEED_TAB3;
@@ -321,19 +339,21 @@ static unsigned char getSpeedIndex(pECBMotDriv self, int value){
}
for(index = 0; index < length-1; index++){
if(table[index] <= value){
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;
int accel, status, actual;
parameter = getSpeedIndex(self,value);
parameter = getSpeedIndex(value, (int)rint(ObVal(self->driverPar,RANGE)),
&actual);
in.c = (unsigned char)self->ecbIndex;
in.b = code;
@@ -355,8 +375,8 @@ static int loadDelay(pECBMotDriv self){
int accel, status;
unsigned char control;
parameter = (int)nintf(ObVal(self->driverPar,DELAY));
control = (unsigned char)nintf(ObVal(self->driverPar,CONTROL));
parameter = (int)rint(ObVal(self->driverPar,DELAY));
control = (unsigned char)rint(ObVal(self->driverPar,CONTROL));
if(control & 3){
parameter = 5;
} else{
@@ -379,16 +399,16 @@ static int loadDelay(pECBMotDriv self){
}
/*---------------------------------------------------------------------*/
static int loadMulti(pECBMotDriv self){
unsigned char multi, mult_chan;
int multi, mult_chan;
Z80_reg in, out;
int status;
multi = nintf(ObVal(self->driverPar,MULT));
multi = rint(ObVal(self->driverPar,MULT));
if(multi <= 0){
return 1; /* not multiplexed */
}
mult_chan = (unsigned char)nintf(ObVal(self->driverPar,MULTCHAN));
mult_chan = (unsigned char)rint(ObVal(self->driverPar,MULTCHAN));
in.b = -1; /* SET_PORT */
in.d = (unsigned char)(multi + (mult_chan << 4));
in.e = self->ecbIndex;
@@ -399,6 +419,33 @@ static int loadMulti(pECBMotDriv self){
}
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;
}
/*----------------------------------------------------------------------*/
static int downloadECBParam(pECBMotDriv self){
int status, parameter;
@@ -437,6 +484,15 @@ static int downloadECBParam(pECBMotDriv self){
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
@@ -461,6 +517,65 @@ int degree2Step(pECBMotDriv self, float degree)
}
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 motor
*/
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);
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;
@@ -496,6 +611,20 @@ static int ECBRunTo(void *pData, float newPosition){
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
*/
@@ -504,7 +633,7 @@ static int ECBRunTo(void *pData, float newPosition){
in.d |= 32; /* positive direction */
}
in.d |= 16; /* interrupts */
if(nintf(ObVal(self->driverPar,RANGE)) == 1.){
if(rint(ObVal(self->driverPar,RANGE)) == 1.){
in.d |= 64; /* fast speed */
}
in.c = (unsigned char)self->ecbIndex;
@@ -517,15 +646,18 @@ static int ECBRunTo(void *pData, float newPosition){
/*
calculate steps
*/
self->restart = 0;
backlash = ObVal(self->driverPar,BACKLASH);
if(diff < 0){
diff = -diff;
if(backlash > 0){
if(backlash > 0.){
diff += backlash;
self->restart = 1;
}
} else {
if(backlash < 0){
if(backlash < 0.){
diff -= backlash;
self->restart = 1;
}
}
data.result = degree2Step(self,diff);
@@ -550,10 +682,36 @@ static int ECBRunTo(void *pData, float newPosition){
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 & 64){
self->errorCode = ECBINHIBIT;
return HWFault;
} 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;
int status, result;
assert(self);
@@ -565,17 +723,24 @@ static int ECBGetStatus(void *pData){
return HWFault;
}
status = checkMotorResult(self,out);
if(status == 0){
if(self->errorCode == ECBRUNNING){
self->errorCode = 0;
return HWBusy;
} else {
return HWFault;
}
} else {
return HWIdle;
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){
@@ -622,7 +787,7 @@ static int ECBTryAndFixIt(void *pData, int iCode, float fNew){
switch(iCode){
case ECBMANUELL:
in.d =1 ;
in.d = 1 ;
ecbExecute(self->ecb,162,in,&out);
result = MOTREDO;
break;
@@ -644,13 +809,15 @@ static int ECBHalt(void *pData){
assert(self);
if(nintf(ObVal(self->driverPar,RANGE)) == 1){
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;
}
/*=======================================================================*/
@@ -669,17 +836,88 @@ static int ECBGetDriverPar(void *pData,char *name, float *value){
}
}
/*=====================================================================*/
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;
int parNumber, speedNumber, actualSpeed, status;
char pBueffel[256];
float correctedValue;
parNumber = ObParIndex(self->driverPar,name);
if(parNumber < 0){
return 0;
}
assert(self);
/*
only managers shall edit these parameters....
@@ -687,9 +925,255 @@ static int ECBSetDriverPar(void *pData, SConnection *pCon, char *name,
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;
}
}
/*
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);
assert(self);
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++){
sprintf(pBueffel,"%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,MAXPAR-1,"tueet",-100,-100); /* sentinel! */
}
/*------------------------------------------------------------------------*/
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->errorCode = 0;
return (MotorDriver *)self;
}