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

pumatask()
{
	TRSF_PTR z, e , b1, s1, s2, s3, conv1, conv2;
	POS_PTR  p0;
	POS_PTR p1[4], p2[4];
	int convfn();
	int i, j;

	conv1 = newtrans("CONV1",convfn);
	conv2 = newtrans("CONV2",convfn);
	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_rot("B1", 600. , -400., 500., yunit, 180.);
	s1 = gentr_trsl("S1", 0., 0., 80.);
	s2 = gentr_trsl("S2", 80., 0., 80.);
	s3 = gentr_trsl("S3", 80., 0., 0.);

	p0 = makeposition("P0" , z, t6, e, EQ, b1, TL, e);
	p1[0] = makeposition("P10" , z, t6, e, EQ, conv1, b1, TL, e);
	p1[1] = makeposition("P11" , z, t6, e, EQ, conv1, b1, s1, TL, e);
	p1[2] = makeposition("P12" , z, t6, e, EQ, conv1, b1, s2, TL, e);
	p1[3] = makeposition("P13" , z, t6, e, EQ, conv1, b1, s3, TL, e);
	p2[0] = makeposition("P20" , z, t6, e, EQ, conv2, b1, TL, e);
	p2[1] = makeposition("P21" , z, t6, e, EQ, conv2, b1, s1, TL, e);
	p2[2] = makeposition("P22" , z, t6, e, EQ, conv2, b1, s2, TL, e);
	p2[3] = makeposition("P23" , z, t6, e, EQ, conv2, b1, s3, TL, e);


	movejnts(p0, 100, 1500);
	rtime = 0;
	for (j = 0; j < 6; ++j) {
		for (i = 0; i < 4; ++i) {
			movecart(p1[i], 100, 300);
		}
	}
	movejnts(p0, 100, 1500);
	waitfor(completed);
	rtime = 0;
	for (j = 0; j < 6; ++j) {
		for (i = 0; i < 4; ++i) {
			movejnts(p2[i], 100, 300);
		}
	}
	stop(0);
	movejnts(park, 100, 1500);
}

convfn(t)
TRSF_PTR t;
{
	t->p.y = .08 * rtime;
}
