- First working version of counter driver for ECB
- Motor driver works for normal, control and encoder motors
This commit is contained in:
cvs
520
ecbcounter.c
Normal file
520
ecbcounter.c
Normal file
@@ -0,0 +1,520 @@
|
||||
/*----------------------------------------------------------------------------
|
||||
This is a single counter implemented on top of the Risoe ECB electronic
|
||||
|
||||
copyright: see file COPYRIGHT
|
||||
|
||||
Mark Koennecke, January-February 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 "ecb.h"
|
||||
#include "countdriv.h"
|
||||
|
||||
/*------------------ our private data structure ------------------------*/
|
||||
typedef struct {
|
||||
pECB ecb; /* the ECB system we talk to */
|
||||
unsigned char prescaler[8]; /* an array for the prescaler values */
|
||||
int tfreq; /* timer frequency */
|
||||
unsigned char control; /* marks the control monitor */
|
||||
}ECBCounter, *pECBCounter;
|
||||
|
||||
/*----------------- private defines ------------------------------------*/
|
||||
#define STFRD 137
|
||||
#define STREAD 138
|
||||
#define STOPS 136
|
||||
#define STCLEA 134
|
||||
#define PRELOA 139
|
||||
#define STLOAD 156
|
||||
#define STCPRE 133
|
||||
#define STARTS 135
|
||||
|
||||
#define MAX_COUNT 4294967295.0
|
||||
/*------------------ error codes --------------------------------------*/
|
||||
#define COMMERROR -300
|
||||
#define TOMANYCOUNTS -301
|
||||
#define NOSEND -302
|
||||
#define INVALIDCOUNTER -304
|
||||
#define INVALIDPRESCALER -305
|
||||
#define BADFREQ -306
|
||||
/*======================================================================*/
|
||||
static int readScaler(pECBCounter pPriv, int scaler, int *count){
|
||||
int status;
|
||||
Z80_reg in, out;
|
||||
Ecb_pack data;
|
||||
|
||||
in.c = (unsigned char)scaler;
|
||||
status = ecbExecute(pPriv->ecb,STREAD,in,&out);
|
||||
if(status != 1){
|
||||
return COMMERROR;
|
||||
}
|
||||
|
||||
data.b.byt3 = out.c;
|
||||
data.b.byt2 = out.b;
|
||||
data.b.byt1 = out.d;
|
||||
data.b.byt0 = out.e;
|
||||
if(scaler == 0){
|
||||
*count = data.result/pPriv->tfreq;
|
||||
} else {
|
||||
*count = data.result;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int ECBGetStatus(struct __COUNTER *self, float *fControl){
|
||||
pECBCounter pPriv = (pECBCounter)self->pData;
|
||||
int status, result, scaler;
|
||||
Z80_reg in, out;
|
||||
int count;
|
||||
|
||||
assert(pPriv);
|
||||
|
||||
/*
|
||||
read status bit
|
||||
*/
|
||||
status = ecbExecute(pPriv->ecb,STFRD,in,&out);
|
||||
if(status != 1){
|
||||
self->iErrorCode = COMMERROR;
|
||||
return HWFault;
|
||||
}
|
||||
if(out.d == 0){
|
||||
result = HWIdle;
|
||||
} else {
|
||||
result = HWBusy;
|
||||
}
|
||||
|
||||
/*
|
||||
select which scaler to read
|
||||
*/
|
||||
if(self->eMode == eTimer){
|
||||
scaler = 0;
|
||||
}else {
|
||||
scaler = pPriv->control;
|
||||
}
|
||||
|
||||
readScaler(pPriv,scaler,&count);
|
||||
/*
|
||||
ignore errors on this one
|
||||
*/
|
||||
*fControl = (float)count;
|
||||
|
||||
return result;
|
||||
}
|
||||
/*======================================================================*/
|
||||
static int stopScalers(pECBCounter self){
|
||||
int status;
|
||||
Z80_reg in, out;
|
||||
|
||||
status = ecbExecute(self->ecb,STOPS,in,&out);
|
||||
if(status != 1){
|
||||
return COMMERROR;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int clearScalers(pECBCounter self){
|
||||
int status;
|
||||
Z80_reg in, out;
|
||||
|
||||
status = ecbExecute(self->ecb,STCLEA,in,&out);
|
||||
if(status != 1){
|
||||
return COMMERROR;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int loadPrescalers(pECBCounter self){
|
||||
Z80_reg in, out;
|
||||
int status, i;
|
||||
|
||||
for(i = 0; i < 8; i++){
|
||||
in.c = (unsigned char)i;
|
||||
in.d = self->prescaler[i];
|
||||
status = ecbExecute(self->ecb,PRELOA,in,&out);
|
||||
if(status != 1){
|
||||
return COMMERROR;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*----------------------------------------------------------------------*/
|
||||
static int loadPreset(pECBCounter self, int preset, unsigned char control){
|
||||
Z80_reg in, out;
|
||||
Ecb_pack data;
|
||||
int status, i;
|
||||
|
||||
data.result = preset;
|
||||
|
||||
in.c = data.b.byt3;
|
||||
in.b = data.b.byt2;
|
||||
in.e = data.b.byt1;
|
||||
in.d = data.b.byt0;
|
||||
status = ecbExecute(self->ecb,STLOAD,in,&out);
|
||||
if(status != 1){
|
||||
return COMMERROR;
|
||||
}
|
||||
|
||||
in.b = data.b.byt2;
|
||||
in.e = data.b.byt1;
|
||||
in.d = data.b.byt0;
|
||||
in.c = 4*control;
|
||||
status = ecbExecute(self->ecb,STCPRE,in,&out);
|
||||
if(status != 1){
|
||||
return COMMERROR;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int ECBStart(struct __COUNTER *self){
|
||||
pECBCounter pPriv = NULL;
|
||||
int preset, status, controlUnit;
|
||||
Z80_reg in, out;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
/*
|
||||
check if the preset is permissible
|
||||
*/
|
||||
preset = (int)rint(self->fPreset);
|
||||
if(preset > MAX_COUNT){
|
||||
self->iErrorCode = TOMANYCOUNTS;
|
||||
return HWFault;
|
||||
}
|
||||
if(self->eMode == eTimer){
|
||||
controlUnit = 0;
|
||||
preset *= pPriv->tfreq;
|
||||
if(preset > MAX_COUNT){
|
||||
self->iErrorCode = TOMANYCOUNTS;
|
||||
return HWFault;
|
||||
}
|
||||
} else {
|
||||
controlUnit = pPriv->control;
|
||||
}
|
||||
|
||||
if((status = stopScalers(pPriv)) <= 0){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
if((status = clearScalers(pPriv)) <= 0){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
if((status = loadPrescalers(pPriv)) <= 0){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
if((status = loadPreset(pPriv, preset,(unsigned char)controlUnit)) <= 0){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
status = ecbExecute(pPriv->ecb,STARTS,in,&out);
|
||||
if(status != 1){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
return OKOK;
|
||||
}
|
||||
/*========================================================================
|
||||
These two functions currently rely on the idea that the ECB stops
|
||||
and starts without clearing counters in between. The sequence of
|
||||
things necessary to start it, suggests this. If this is not the case then
|
||||
this will not work.
|
||||
===========================================================================*/
|
||||
static int ECBPause(struct __COUNTER *self){
|
||||
int status;
|
||||
pECBCounter pPriv = NULL;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
if((status = stopScalers(pPriv)) <= 0){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*=======================================================================*/
|
||||
static int ECBContinue(struct __COUNTER *self){
|
||||
int status;
|
||||
pECBCounter pPriv = NULL;
|
||||
Z80_reg in, out;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
status = ecbExecute(pPriv->ecb,STARTS,in,&out);
|
||||
if(status != 1){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
return OKOK;
|
||||
}
|
||||
/*=======================================================================*/
|
||||
static int ECBHalt(struct __COUNTER *self){
|
||||
int status;
|
||||
pECBCounter pPriv = NULL;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
if((status = stopScalers(pPriv)) <= 0){
|
||||
self->iErrorCode = status;
|
||||
return HWFault;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*=======================================================================*/
|
||||
static int ECBTransfer(struct __COUNTER *self){
|
||||
int status, count, i;
|
||||
pECBCounter pPriv = NULL;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
/*
|
||||
read time
|
||||
*/
|
||||
status = readScaler(pPriv,0,&count);
|
||||
if(status <= 0){
|
||||
self->iErrorCode = COMMERROR;
|
||||
return HWFault;
|
||||
}
|
||||
self->fTime = (float)count;
|
||||
|
||||
/*
|
||||
read other scalers
|
||||
*/
|
||||
for(i = 1; i < 8; i++){
|
||||
status = readScaler(pPriv,i,&count);
|
||||
if(status <= 0){
|
||||
self->iErrorCode = COMMERROR;
|
||||
return HWFault;
|
||||
}
|
||||
self->lCounts[i-1] = count;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
/*======================================================================*/
|
||||
static int ECBGetError(struct __COUNTER *self, int *iCode,
|
||||
char *errorText, int errlen){
|
||||
char pBueffel[132];
|
||||
|
||||
*iCode = self->iErrorCode;
|
||||
switch(self->iErrorCode){
|
||||
case COMMERROR:
|
||||
strncpy(errorText,"Communication error with ECB",errlen);
|
||||
break;
|
||||
case TOMANYCOUNTS:
|
||||
strncpy(errorText,"Preset is to high!",errlen);
|
||||
break;
|
||||
case NOSEND:
|
||||
strncpy(errorText,"Cannot send naked data to ECB",errlen);
|
||||
break;
|
||||
case UNKNOWNPAR:
|
||||
strncpy(errorText,"parameter unknown",errlen);
|
||||
break;
|
||||
case INVALIDCOUNTER:
|
||||
strncpy(errorText,"Invalid counter number requested, 0-7 allowed",
|
||||
errlen);
|
||||
break;
|
||||
case INVALIDPRESCALER:
|
||||
strncpy(errorText,"Invalid prescaler value, allowed 1 or 10",
|
||||
errlen);
|
||||
break;
|
||||
case BADFREQ:
|
||||
strncpy(errorText,"Bad timer frequency: 10 or 1000 allowed",errlen);
|
||||
break;
|
||||
default:
|
||||
sprintf(pBueffel,"Unknown error code %d", self->iErrorCode);
|
||||
strncpy(errorText,pBueffel,errlen);
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
/*=======================================================================*/
|
||||
static int ECBFixIt(struct __COUNTER *self, int iCode){
|
||||
return COTERM;
|
||||
}
|
||||
/*======================================================================*/
|
||||
|
||||
/*******************************************************************************
|
||||
* Load the parameters 'dot' and 'divide' for a motor or an encoder.
|
||||
* 'dot' specifies the placement of a punctuation mark on the display
|
||||
* of f.ex a motor position. 'divide' specifies how many times the po-
|
||||
* sition is to be divided by two before it is displayed.
|
||||
******************************************************************************/
|
||||
static void
|
||||
Dot_divide (int device, int data, pECB ecb)
|
||||
{
|
||||
int function, dot, divide;
|
||||
Z80_reg x_inreg, out;
|
||||
|
||||
if (data == 0) /* If zero, dont send dot/divide) */
|
||||
return;
|
||||
|
||||
dot = 0;
|
||||
while ((data%10) == 0)
|
||||
{
|
||||
dot++;
|
||||
data /= 10;
|
||||
}
|
||||
divide = 0;
|
||||
while ((data%2) == 0)
|
||||
{
|
||||
divide++;
|
||||
data /= 2;
|
||||
}
|
||||
if (data != 1) /* If != 1, not a binary No. */
|
||||
return;
|
||||
if (dot > 0)
|
||||
dot = 8 - dot;
|
||||
x_inreg.c = 0; /* Specify input */
|
||||
x_inreg.b = (unsigned char) device;
|
||||
x_inreg.d = (unsigned char) dot; /* Dot position */
|
||||
x_inreg.e = (unsigned char) divide; /* No. of times to divide by 2 */
|
||||
|
||||
ecbExecute(ecb,170,x_inreg,&out);
|
||||
return;
|
||||
}
|
||||
/*-----------------------------------------------------------------------*/
|
||||
static int ECBSet(struct __COUNTER *self, char *name,
|
||||
int iCter, float fVal){
|
||||
pECBCounter pPriv = NULL;
|
||||
int iVal;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
iVal = (int)rint(fVal);
|
||||
|
||||
if(strcmp(name,"prescaler") == 0){
|
||||
if(iCter < 0 || iCter > 7){
|
||||
self->iErrorCode = INVALIDCOUNTER;
|
||||
return HWFault;
|
||||
}
|
||||
if(iVal != 1 && iVal != 10){
|
||||
self->iErrorCode = INVALIDPRESCALER;
|
||||
return HWFault;
|
||||
}
|
||||
pPriv->prescaler[iCter] = (unsigned char)iVal;
|
||||
return OKOK;
|
||||
} else if(strcmp(name,"tfreq") == 0){
|
||||
if(fVal == 1000){
|
||||
pPriv->prescaler[0] = 1;
|
||||
pPriv->tfreq = 1000;
|
||||
Dot_divide(64,1000,pPriv->ecb);
|
||||
return OKOK;
|
||||
} else if(fVal == 10){
|
||||
pPriv->tfreq = 10;
|
||||
pPriv->prescaler[0] = 10;
|
||||
Dot_divide(64,10,pPriv->ecb);
|
||||
return OKOK;
|
||||
} else {
|
||||
self->iErrorCode = BADFREQ;
|
||||
return HWFault;
|
||||
}
|
||||
} else {
|
||||
self->iErrorCode = UNKNOWNPAR;
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
/*===================================================================*/
|
||||
static int ECBGet(struct __COUNTER *self, char *name,
|
||||
int iCter, float *fVal){
|
||||
pECBCounter pPriv = NULL;
|
||||
|
||||
assert(self);
|
||||
pPriv = (pECBCounter)self->pData;
|
||||
assert(pPriv);
|
||||
|
||||
if(strcmp(name,"prescaler") == 0){
|
||||
*fVal = (float)pPriv->prescaler[iCter];
|
||||
return OKOK;
|
||||
} else if(strcmp(name,"tfreq") == 0){
|
||||
*fVal = (float)pPriv->tfreq;
|
||||
return OKOK;
|
||||
} else{
|
||||
self->iErrorCode = UNKNOWNPAR;
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
/*=====================================================================*/
|
||||
static int ECBSend(struct __COUNTER *self, char *text,
|
||||
char *reply, int replylen){
|
||||
strncpy(reply,"ECB does not feast on ASCII strings, refused!",
|
||||
replylen);
|
||||
return OKOK;
|
||||
}
|
||||
/*====================================================================*/
|
||||
pCounterDriver MakeECBCounter(char *ecb){
|
||||
pECBCounter pPriv = NULL;
|
||||
pCounterDriver self = NULL;
|
||||
int i;
|
||||
|
||||
/*
|
||||
memory for everybody
|
||||
*/
|
||||
self = CreateCounterDriver("ecb","ecb");
|
||||
pPriv = (pECBCounter)malloc(sizeof(ECBCounter));
|
||||
if(self == NULL || pPriv == NULL){
|
||||
return NULL;
|
||||
}
|
||||
memset(pPriv,0,sizeof(ECBCounter));
|
||||
|
||||
/*
|
||||
initialize private data structure
|
||||
*/
|
||||
pPriv->ecb = (pECB)FindCommandData(pServ->pSics,ecb,"ECB");
|
||||
if(pPriv->ecb == NULL){
|
||||
DeleteCounterDriver(self);
|
||||
free(pPriv);
|
||||
return NULL;
|
||||
}
|
||||
for(i = 0; i < 8; i++){
|
||||
pPriv->prescaler[i] = 1;
|
||||
}
|
||||
pPriv->tfreq = 1000;
|
||||
pPriv->control = 1;
|
||||
|
||||
|
||||
/*
|
||||
assign function pointers
|
||||
*/
|
||||
self->GetStatus = ECBGetStatus;
|
||||
self->Start = ECBStart;
|
||||
self->Pause = ECBPause;
|
||||
self->Continue = ECBContinue;
|
||||
self->Halt = ECBHalt;
|
||||
self->ReadValues = ECBTransfer;
|
||||
self->GetError = ECBGetError;
|
||||
self->TryAndFixIt = ECBFixIt;
|
||||
self->Set = ECBSet;
|
||||
self->Get = ECBGet;
|
||||
self->Send = ECBSend;
|
||||
|
||||
self->pData = pPriv;
|
||||
return self;
|
||||
}
|
||||
/*=====================================================================*/
|
||||
void KillECBCounter(struct __COUNTER *self){
|
||||
DeleteCounterDriver(self);
|
||||
}
|
||||
Reference in New Issue
Block a user