/*
 * RTC  Version 2.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : rtc
 *      File    : random.c
 *      Remarks : Utility, for tests
 *      Usage   : cc random.c rtc.a; a.out
 */

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

#define ACC     6

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

	stepmode();

	control(dummy, back);
	for (; ; )
		pause();
}


static back()
{
	extern struct how how;
	extern struct chg chg;
	extern int terminate;
	static int first = YES;
	static unsigned short   where[NJOINTS],
				inc[NJOINTS],
				half[NJOINTS],
				cal[NJOINTS] =
			{ECCL1, ECCL2, ECCL3, ECCL4, ECCL5, ECCL6};
	register int i;

	if (first) {
		first = NO;
		for (i = 0; i < NJOINTS; ++i) {
			where[i] = how.pos[i];
			if (where[i] > cal[i]) {
				half[i] = (where[i] - cal[i]) / 2 + cal[i];
			}
			else {
				half[i] = cal[i] - (cal[i] - where[i]) / 2;
			}
			inc[i] = 0;
		}
		return;
	}
	for (i = 0; i < NJOINTS; ++i) {
		if (where[i] > cal[i]) {
			if (where[i] > half[i]) {
				  inc[i] -= ACC;
			}
			else {
				  inc[i] += ACC;
			}
		}
		if (where[i] < cal[i]) {
			if (where[i] < half[i]) {
				  inc[i] += ACC;
			}
			else {
				  inc[i] -= ACC;
			}
		}
		where[i] += inc[i];
		chg.a_motion.vala[i] = where[i];
	}
	chg.a_motion.set = POS;
}


dummy(){}
