home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl064
< prev
next >
Wrap
Text File
|
1987-03-02
|
3KB
|
168 lines
/*
* MAC Version 2.0 Author : Vincent Hayward
* School of Electrical Engineering
* Purdue University
* Dir : mac
* File : mcal.c
* Remarks : calibration function
* Usage : part of maint and moper
*/
#include "../h/which.h"
#include "../h/addefs.h"
#include "../h/jcom.h"
#ifdef VALII
#define TACK 8
#else
#define TACK 4
#endif
#define YES 1
#define NO 0
extern short (* onclk)();
extern short tick;
unsigned short index[NJOINTS];
unsigned short calpos[NJOINTS];
static short timeout;
static short calstep;
static unsigned short calrefp[NJOINTS], calidxp[NJOINTS], off[NJOINTS];
cal(refp, idxp)
unsigned short *refp, *idxp;
{
short armcal(), steper(), refarm();
register short i;
tick = 0;
for (i = 0; i < NJOINTS; ++i) {
calrefp[i] = refp[i];
calidxp[i] = idxp[i];
off[i] = YES;
}
put6j(STOPM, calrefp);
get6j(READ, calpos);
if (!getio(ARMPWR)) {
bisio(ARMPWR);
while(!getio(ARMPWR))
;
}
put6pg(MODE, MESERVO|MEPOSSV);
get6j(READ, calpos);
onclk = steper;
timeout = 7;
calstep = -16;
tick = TACK;
while(tick) {
wait();
}
put6pg(MODE, MESERVO|MEPOSSV|MLOOKZI);
onclk = armcal;
timeout = 250;
calstep = 16;
tick = TACK;
while(tick) {
wait();
}
if(timeout) {
put6j(STOPM, calidxp);
for (i = 0; i < NJOINTS; ++i) {
calpos[i] = calidxp[i];
}
onclk = refarm;
calstep = 16;
tick = TACK;
while (tick) {
wait();
}
put6j(POSET, calrefp);
return(YES);
}
else {
return(NO);
}
}
short armcal()
{
register short i = NJOINTS;
unsigned short stat[NJOINTS];
if (--timeout) {
get6j(JSTAT, stat);
while (i > 0 && stat[--i] & JFNDIXS)
;
if (i) {
for(i = 0; i < NJOINTS; ++i)
calpos[i] += calstep;
put6j(POSET, calpos);
}
else {
tick = 0;
}
}
else {
tick = 0;
}
}
short steper()
{
register short i;
if (--timeout) {
for(i = 0; i < NJOINTS; ++i)
calpos[i] += calstep;
put6j(POSET, calpos);
}
else {
tick = 0;
}
}
short refarm()
{
short i, done = YES;
for (i = 0; i < NJOINTS; ++i) {
if (off[i]) {
done = NO;
}
else {
continue;
}
if (calidxp[i] > calrefp[i]) {
calpos[i] -= calstep;
if (calpos[i] < calrefp[i]) {
off[i] = NO;
}
}
if (calidxp[i] < calrefp[i]) {
calpos[i] += calstep;
if (calpos[i] > calrefp[i]) {
off[i] = NO;
}
}
if (calidxp[i] == calrefp[i]) {
off[i] = NO;
}
}
put6j(POSET, calpos);
if (done) {
tick = 0;
}
}