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


pumatask()
{
	TRSF_PTR z, b1, e, h, search, track, pt, over;
	POS_PTR  p1, p2, p3, get;
	int searchfn(), trackfn();

	search = newtrans("FING",searchfn);
	track = newtrans("FING",trackfn);
	h = newtrans("H", const);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
	e = gentr_eul("E" , 0. , 0. , 200. , 0. , 0.,  0.);
	b1 = gentr_rot("B1", 600. ,-200., 450., yunit, 180.);
	pt = gentr_rot("PT", 0., 0., 0., zunit, 90.);
	over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);

	p1 = makeposition("P1" , z, t6, e, EQ, b1, pt, TL, e);
	p2 = makeposition("P2" , z, t6, e, h, search, EQ, b1, pt, TL, e);
	p3 = makeposition("P3" , z, t6, e, h, track, EQ, b1, pt, TL, e);
	get = makeposition("GET", z, t6, EQ, over, TL, t6);

	movecart(get, 200, 1500);
	movecart(p1, 200, 1500);
	movecart(p2, 200, 1500);
	movecart(p3, 100, 5000);
	movejnts(park, 200, 1500);
}


searchfn(t)
TRSF_PTR t;
{
	t->p.z -= 1.;
}

trackfn(t)
TRSF_PTR t;
{
	t->p.x -= 1.;
	t->p.y -= 1.;
	t->p.z -= 1.;
	rotm(t, xunit, .1);
	rotm(t, yunit, .1);
	rotm(t, zunit, .1);
}
