#include "../h/rccl.h"
#include "../h/umac.h"

pumatask()
{
	TRSF_PTR z, e, conv, or, fl;
	POS_PTR  pm;
	int convfn();
	int i;

	conv = newtrans("CONV",convfn);
	z = gentr_trsl("Z",  0.,  0., 864.);
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	or = gentr_eul("OR", 000. , 600., 500.,    0., 0., 90.);
	fl = gentr_rot("FL", 0. , 0., 0., yunit, 180.);

	pm = makeposition("PM" , z, t6, e, EQ, conv, or, fl, TL, e);

	movecart(pm, 300, 1500);
	for (; ; ) {
		printf("cart ");QUERY(i);
		if (i == 'y') {
			for (i = 0; i < 8; ++i) {
				movecart(pm, 100, 500);
				movecart(pm, 100, 500);
			}
		}
		printf("jnts ");QUERY(i);
		if (i == 'y') {
			for (i = 0; i < 8; ++i) {
				movejnts(pm, 100, 500);
				movejnts(pm, 100, 500);
			}
		}
		printf("mixt ");QUERY(i);
		if (i == 'y') {
			for (i = 0; i < 8; ++i) {
				movejnts(pm, 100, 500);
				movecart(pm, 100, 500);
			}
		}
		printf("quit ");QUERY(i);
		if (i == 'y') {
			break;
		}
	}
	movecart(park, 300, 1500);
}



convfn(t)
TRSF_PTR t;
{
	extern int rtime;
	double time;

	double omega = .125;
	double radius = 200.;

	time = rtime / 1000.;
	t->p.x = radius * cos(omega * time * PIT2);
	t->p.z = radius * sin(omega * time * PIT2);
}
