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

pumatask()
{
	TRSF_PTR z, e , b1, rt;
	POS_PTR  p1, p2;
	int q;

	z = gentr_rot("Z",  0.,  0., 864., &zun, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	b1 = gentr_pao("B1", -128. , 800.,  900., 0., 0., -1., 0., 1., 0.);
	rt = rot(newtrans("RT", const), xunit, 90.);

	p1   = makeposition("P1" , z, t6, e, EQ, b1, TL, e);
	p2   = makeposition("P2" , z, t6, e, rt, EQ, b1, TL, e);

	movejnts(p1, 300, 2000);
	for (; ; ) {
		QUERY(q); if (q == 'n') break;
		/*
		 * arm is lun, go luf
		 */
		moveconf(p2, 300, 1000, "f");
		QUERY(q); if (q == 'n') break;
		/*
		 * arm is luf, go ldn
		 */
		moveconf(p2, 300, 2000, "dn");
		QUERY(q); if (q == 'n') break;
		/*
		 * arm is ldn, go lun
		 */
		moveconf(p1, 300, 2000, "u");
	}
	moveconf(park, 300, 2000, "lun");
	printf("END\n");
}
