/*
 * 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();
}
