home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl071
< prev
next >
Wrap
Text File
|
1987-03-02
|
5KB
|
321 lines
#include "../h/which.h"
#include "../h/addefs.h"
#include "../h/jcom.h"
#include "../h/unim.h"
#include "../h/rtc.h"
#include "../h/adc.h"
#define HNDPROC 6
static short ibuff[COMSIZE]; /* input buffer */
static struct how how; /* output buffer */
static short ref[NJOINTS];
static short idx[NJOINTS];
static short calreq;
static short first;
#ifdef ADC
static short adcmap[ADC];
static short nch;
#endif
#ifdef VALII
static short rate[6] = {2, 4, 8, 16, 32, 64};
#else
static short rate[6] = {1, 2, 4, 8, 16, 32};
#endif
static short ninter[6] = {8, 16, 32, 64, 128, 256};
static short intscl[6] = {5, 4, 3, 2, 1, 0};
#define GETSHORT value = *scan++; value <<= 8; value |= *scan++ & 0xff;
short drive()
{
register char *scan = (char *)ibuff;
register unsigned short value = 0;
register short i = 0;
unsigned short chks = 0;
short wc, dr11();
short *sp;
unsigned short set[6];
extern short tick;
/*
* gather data
*/
how.exio = getio(-1);
get6j(READ, how.pos);
#ifdef ADC
for (i = 0; i < ADC; ++i) {
if (adcmap[i] >= 0) {
ADCV(how.adcr[adcmap[i]], i);
}
}
#endif
sp = (short *)how.pos;
chks = how.exio;
for (i = 0; i < NJOINTS; ++i) {
chks += *sp++;
}
#ifdef ADC
for (i = 0; i < nch; ++i) {
chks += *sp++;
}
#endif
*sp = chks;
/*
* interrupt vax send data, get commands
*/
#ifdef ADC
if ((wc = dr11(ibuff, &how, 1 + NJOINTS + nch + 1)) < 0) {
zexit(wc);
}
#else
if ((wc = dr11(ibuff, &how, 1 + NJOINTS + 1)) < 0) {
zexit(wc);
}
#endif
if (wc == 0) {
return;
}
/*
* analyze commands and operate the arm
*/
if (first) {
first = NO;
if (ibuff[0] != VERSION)
zexit(13);
if (ibuff[1] != ARMTYPE)
zexit(14);
#ifdef STAN
putp(MODE, MESERVO, 6);
#endif
for (i = 0; i < NJOINTS; ++i) {
ref[i] = ibuff[i + 2];
idx[i] = ibuff[i + 2 + NJOINTS];
}
put6j(STOPP, /* dummy arg to satisfy macro */ set);
put6pg(MODE, MESERVO);
#ifdef PUMA
putp(MODE, MESERVO | MEHLFCT, 5);
#endif
return;
}
i = wc - 1;
sp = ibuff;
chks = 0;
while(i--) {
chks += *sp++;
}
if (*sp != chks) {
zexit(12);
}
*sp = -1;
scan = (char *)ibuff;
while (*scan != -1) {
if ((i = (chks = *scan) & 0x0f) <= CMD_I) {
++scan;
GETSHORT
switch (chks & 0x0f0) {
case STOPP_I :
putj(STOPP, value, i);
break;
case STOPM_I :
putj(STOPM, value, i);
break;
case POS_I :
putj(POSET, value, i);
break;
case CUR_I :
putj(DACSET, value, i);
break;
default :
zexit(15);
}
continue;
}
switch ((chks = *scan) & 0x0f) {
case CMD_A :
scan++;
GETSHORT
set[0] = value;
GETSHORT
set[1] = value;
GETSHORT
set[2] = value;
GETSHORT
set[3] = value;
GETSHORT
set[4] = value;
GETSHORT
set[5] = value;
switch (chks & 0xff) {
case STOPP_A :
put6j(STOPP, set);
break;
case STOPM_A :
put6j(STOPM, set);
break;
case POS_A :
put6j(POSET, set);
break;
case CUR_A :
put6j(DACSET, set);
break;
default :
zexit(16);
}
break;
case CMD_G :
++scan;
GETSHORT
switch (chks & 0xff) {
#ifdef ADC
case ADCO_G :
adcmap[value] = nch++;
break;
#endif
case HAND_G :
#ifdef PUMA
if(value == 'o') {
bicio(HNDOH);
}
if (value == 'c') {
bisio(HNDOH);
}
#endif
#ifdef STAN
putj(DACSET, value, HNDPROC);
#endif
break;
case RATE_G :
if (value < 6) {
tick = rate[value];
put6pg(NINTER, ninter[value]);
put6pg(INTSCL, intscl[value]);
}
break;
default :
zexit(17);
}
break;
case CMD_E :
switch (*scan & 0xff) {
case STOP_E :
put6jg(STOPP, 0);
break;
case CALIB_E :
calreq = YES;
break;
case END_E :
tick = 0;
break;
default :
zexit(18);
}
scan += 1;
break;
default :
zexit(19);
}
}
}
#ifdef DEBUG
#include "../h/stdio11.h"
#endif
main()
{
extern short (* onclk)();
extern short tick;
extern short drive();
int i;
#ifdef DEBUG
setbuf(stdin, NULL);
setbuf(stdout, NULL);
#endif
for (; ; ) {
first = YES;
calreq = NO;
#ifdef ADC
for (i = 0; i < NJOINTS; ++i) {
adcmap[i] = i;
}
for (i = NJOINTS; i < ADC; ++i) {
adcmap[i] = -1;
}
nch = NJOINTS;
#endif
bicio(ARMPWR);
bisio(ARMPWR);
put6pg(MODE, MESERVO|MEPOSSV);
put6pg(NINTER, ninter[DEFAULTRATE]);
put6pg(INTSCL, intscl[DEFAULTRATE]);
while (!getio(ARMPWR))
;
#ifdef DEBUG
printf("active\n");
#endif
onclk = drive;
tick = rate[DEFAULTRATE];
while (tick)
wait();
#ifdef DEBUG
printf("iddle\n");
#endif
if (calreq) {
if (!cal(ref, idx)) {
zexit(99);
}
}
else {
bicio(ARMPWR);
for (i = 50000; --i; )
;
}
}
}
zexit(n)
{
tick = 0;
*VAXCSR = 0;
bicio(ARMPWR);
#ifdef DEBUG
printf("\nexit %d\n", n);
#endif
halt();
}