home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl065
< prev
next >
Wrap
Text File
|
1987-03-02
|
7KB
|
364 lines
/*
* MAC Version 2.0 Author : Vincent Hayward
* School of Electrical Engineering
* Purdue University
* Dir : mac
* File : maint.c
* Remarks : maintenance program
* Usage : make maint
*/
#include "../h/which.h"
#include "../h/addefs.h"
#include "../h/jcom.h"
#ifdef VALII
#define TACK 8
#else
#define TACK 4
#endif
/* global variables shared with the assembly code */
extern short (* onclk)();
extern short tick;
/****************************************************************************/
/* */
/* manual mode */
/* */
/****************************************************************************/
#define MAXMAG 80
#define HANDSTEP 10
#define YES 1
#define NO 0
#define HOME '\036'
#define UP '\013'
#define DOWN '\012'
#define LEFT '\014'
#define BS '\010'
#define CLEAR '\032'
static short curc[6] = { NO, NO, NO, NO, NO, NO };
static unsigned short poshd = 0;
static unsigned short possetp[6];
static unsigned short readpos[6];
static unsigned short trqsetp[6] = {0, 0, 0, 0, 0, 0};
static short dcount[6];
static short mag = 0;
static short nopower;
maint()
{
short j, c;
tick = 0;
for (j = 0; j < 6; ++j)
curc[j] = NO;
do {
printf("\ndo you want to free a joint (y/n) ");
c = getchar();
putchar('\n');
if (c == 'y') {
printf("joint number [1-6] :");
if ((j = getchar()) <= '6' && j >= '1') {
curc[j -= '1'] = YES;
printf("\njoint %d freed\n", j + 1);
}
}
} while(c == 'y');
manu();
}
manu()
{
short ptfn();
short c, j, sig = 1, more = 1, value;
tick = 0;
printf("\nPRESS RETURN TO ENTER MANUAL MODE ");
while(getchar() != '\n')
;
restart:
tick = 0;
putchar(CLEAR);
put6j(STOPP, possetp);
put6pg(MODE, MESERVO);
get6j(READ, readpos);
#ifdef STAN
putp(SCALE, 3, 2);
putp(IGAIN, 0, 3); /* due to enormous backlash J4 */
putp(VGAIN, 0, 3);
putp(SCALE, 2, 3);
putp(MODE, MESERVO, 6);
#endif
if (!getio(ARMPWR)) {
printf("\nTurn on Arm Power\n");
bisio(ARMPWR);
while(!getio(ARMPWR)) {
get6j(READ, readpos);
wait();
}
}
for (j = 0; j < 6; ++j) {
dcount[j] = 0;
possetp[j] = readpos[j];
}
poshd = 0;
mag = 0;
onclk = ptfn;
tick = TACK;
putchar(CLEAR);
printf(
"\n\n\n\n\n\n\n\n\n\n\n FASTER SLOWER REV JOINTS FREE LOCK HAND EXIT\n");
printf(
" '.' ',' '-' [1-6] 'f' 'l' o/c return\n");
putchar(HOME);
printf("\n\n\n");
for ( c = 0; c < 6 ; ++c)
printf(
"%1d %c %6u\n",c + 1, (curc[c]) ? 'F' : 'L', readpos[c]);
while (more) {
if (nopower)
goto restart;
putchar(HOME);
c = getchar();
putchar(LEFT);
switch (c) {
case '1' :
case '2' :
case '3' :
case '4' :
case '5' :
case '6' :
if (!curc[c -= '1']) {
dcount[c] += (sig) ? mag : -mag;
}
printf("\n\n\n");
for (j = c; j--;)
putchar('\n');
printf(
"%1d %c %6u",c + 1, (curc[c]) ? 'F' : 'L', readpos[c]);
break;
#ifdef PUMA
case 'o' :
poshd = 'o';
break;
case 'c' :
poshd = 'c';
break;
#endif
#ifdef STAN
case 'o' :
poshd += HANDSTEP;
break;
case 'c' :
poshd -= HANDSTEP;
break;
#endif
case '.' :
(mag < MAXMAG) ? ++mag : mag;
break;
case ',' :
(mag) ? --mag : mag;
break;
case '-' :
sig = !sig;
break;
case '\n' :
printf("%c exit ? (y/n) ", HOME);
if (getchar() == 'y')
more = 0;
else
printf("%c ", HOME);
break;
case 'f' :
printf("%c free joint [1-6] :", HOME);
if ((c = getchar()) <= '6' && c >= '0') {
printf("\n\n\n");
if (!curc[c -= '1'])
curc[c] = YES;
for (j = c; j--;)
putchar('\n');
printf("%1d F",c + 1);
}
printf("%c ", HOME);
break;
case 'l' :
printf("%c lock joint [1-6] :", HOME);
if ((c = getchar()) <= '6' && c >= '0') {
printf("\n\n\n");
if (curc[c -= '1'])
curc[c] = NO;
for (j = c; j--;)
putchar('\n');
printf("%1d L",c + 1);
}
printf("%c ", HOME);
default :
break;
}
}
tick = 0;
putchar(CLEAR);
}
short ptfn()
{
register short j;
get6j(READ, readpos);
if (nopower = !getio(ARMPWR)) {
return;
}
for (j = 0; j < 6; ++j) {
if (curc[j]) {
putj(DACSET, trqsetp[j], j);
getj(READ, possetp[j], j);
}
else {
if (dcount[j] > 0) {
if (possetp[j] < MAXMAG) {
possetp[j] = 077777;
putj(STOPM, 077777, j);
}
dcount[j] -= mag;
possetp[j] -= mag;
}
if (dcount[j] < 0) {
if (possetp[j] > 0177777-MAXMAG) {
possetp[j] = 077777;
putj(STOPM, 077777, j);
}
dcount[j] += mag;
possetp[j] += mag;
}
putj(POSET, possetp[j], j);
}
}
#ifdef STAN
putj(DACSET, poshd, 6);
#endif
#ifdef PUMA
if (poshd == 'o') {
bicio(HNDOH);
poshd = NO;
}
if (poshd == 'c') {
bisio(HNDOH);
poshd = NO;
}
#endif
}
printb(i, g)
short i, g;
{
short n = 16;
for (; n--; i<<=1) {
if (n != 15 && n%g == g-1)
putchar(' ');
if (i < 0)
putchar('1');
else
putchar('0');
}
}
#include "../h/stdio11.h"
#include "../h/which.h"
#ifdef PUMA
#include "../h/pumadata.h"
#endif
#ifdef STAN
#include "../h/standata.h"
#endif
static unsigned short ref[6] = {ECCL1, ECCL2, ECCL3, ECCL4, ECCL5, ECCL6};
static unsigned short idx[6] = {XCCL1, XCCL2, XCCL3, XCCL4, XCCL5, XCCL6};
main()
{
extern unsigned short *calpos, *index;
short i;
setbuf(stdout, NULL);
setbuf(stdin, NULL);
printf("\nflip switch to Micro and type <return> to begin ");
while (getchar() != '\n')
;
do {
maint();
printf("\ndo you want to calibrate (y/n) ");
if (getchar() == 'y') {
putchar('\n');
if (cal(ref, idx)) {
printf("\nThe arm is now calibrated\n");
get6j(READ, calpos);
get6j(JSTAT, index);
printf("status position\n");
for (i=0; i < 6; i++) {
printb(index[i], 4);
printf(" %u\n", calpos[i]);
}
}
else {
get6j(JSTAT, index);
for (i=0; i < 6; i++) {
if (index[i] & JLOOKZI) {
printf("No index joint %d\n", i+1);
}
}
printf("Error - try again\n");
for (i=0; i<6; i++) {
printb(index[i], 4);
putchar('\n');
}
putchar('\n');
}
}
printf("\nmore (y/n) ");
} while (getchar() == 'y');
}
#define CLKAD ((unsigned short *)0100)
#define CLKPS ((unsigned short *)0102)
zexit(n)
{
tick = 0;
*CLKPS = 2;
*CLKAD = CLKPS;
bicio(ARMPWR);
printf("\nexit %d\n", n);
halt();
}