/*
 * RTC  Version 2.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : rtc
 *      File    : tfree.c
 *      Remarks : Utility, Force control testing.
 *                better not run with the wrist free.
 *      Usage   : cc tfree.c rtc.a -lnm; a.out
 */

#include "../h/rtc.h"
#include "../h/pumadata.h"

#define DEGTORAD        0.01745329251994330

extern struct how how;
extern struct chg chg;

main()
{
	extern int terminate;
	int dummy(), freej();

	control(dummy, freej);
	for (; ; ) {
		nap(10);
		printf("%6d %6d %6d %6d %6d %6d\n",
how.adcr[0], how.adcr[1], how.adcr[2], how.adcr[3], how.adcr[4], how.adcr[5]);
	}
}


dummy(){}

static freej()
{
	double sin(), cos();

	static int first = YES;
	static unsigned short old[NJOINTS];
	static int shake[6] = {0, 0, 0, 0, 0, 0};
	short cur[NJOINTS];
	double jpos[NJOINTS];
	double jrng[NJOINTS];
	double gtor[NJOINTS];
	double c2, c23, s23, c4, s4, c5, s5;
	double bound = 10. * DEGTORAD;
	register int i;

	if (first) {
		first = NO;
		for (i = 0; i < NJOINTS; ++i) {
			old[i] = how.pos[i];
		}
		chg.g_rate.valg = 1;
		chg.g_rate.set = YES;
		return;
	}

	enctorng(jrng, how.pos);
	rngtoang(jpos, jrng);

	c2 = cos(jpos[1]);
	c23 = cos(jpos[1] + jpos[2]);
	s23 = sin(jpos[1] + jpos[2]);
	c4 = cos(jpos[3]);
	s4 = sin(jpos[3]);
	c5 = cos(jpos[4]);
	s5 = sin(jpos[4]);
	gravload(gtor, c2, c23, s23, c4, s4, c5, s5);

	tortodac(cur, gtor, how.pos, old);

	for (i = 0; i < 6; ++i) {
		cur[i] += shake[i];
		shake[i] = -shake[i];
	}

	if (jrng[0] < bound) {
		cur[0] += 500;
	}
	if (jrng[0] > JRNG1 - bound) {
		cur[0] += -500;
	}

	if (jrng[1] < bound) {
		cur[1] += -500;
	}
	if (jrng[1] > JRNG2 - bound) {
		cur[1] += 500;
	}

	if (jrng[2] < bound) {
		cur[2] += 500;
	}
	if (jrng[2] > JRNG3 - bound) {
		cur[2] += -500;
	}

	if (jrng[3] < bound) {
		cur[3] += -400;
	}
	if (jrng[3] > JRNG4 - bound) {
		cur[3] += 400;
	}

	if (jrng[4] < bound) {
		cur[4] += -400;
	}
	if (jrng[4] > JRNG5 - bound) {
		cur[4] += 400;
	}

	if (jrng[5] < bound) {
		cur[5] += -400;
	}
	if (jrng[5] > JRNG6 - bound) {
		cur[5] += 400;
	}

	chg.i_motion[0].set = CUR;
	chg.i_motion[0].vali = cur[0];

	chg.i_motion[1].set = CUR;
	chg.i_motion[1].vali = cur[1];

	chg.i_motion[2].set = CUR;
	chg.i_motion[2].vali = cur[2];

/*
 *  Here i broke the puma's wrist because it bang into limit
 */

/* Suppression....

	chg.i_motion[3].set = CUR;
	chg.i_motion[3].vali = cur[3];

	chg.i_motion[4].set = CUR;
	chg.i_motion[4].vali = cur[4];

	chg.i_motion[5].set = CUR;
	chg.i_motion[5].vali = cur[5];

till here */

	for (i = 0; i < NJOINTS; ++i) {
		old[i] = how.pos[i];
	}
}




gravload(l, c2, c23, s23, c4, s4, c5, s5) /*::*/
register
double *l;
double c2, c23, s23, c4, s4, c5, s5;
{
	l[0] = 0.;
	l[2] = CP32 * s23 + CP31 * c23;
	l[1] = l[2] + CP21 * c2;
	l[3] = -(CP50 * s23 * s4 * s5);
	l[4] = CP50 * (s23 * c4 * c5 + c23 * s5);
	l[5] = 0.;
}
