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

extern FILE *fpi;

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

	conv = newtrans("CONV",convfn);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	or = gentr_eul("OR", 500. , 300., 500.,    10., 0., 0.);
	fl = gentr_rot("FL", 0. , 0., 0., yunit, 180.);

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


/* 1 */ fprintf(fpi,"1\n");
	movecart(pf, 300, 2000);
/* 2 */ fprintf(fpi,"2\n");
	movecart(pm, 300, 5000);
/* 3 */ fprintf(fpi,"3\n");
	movecart(pm, 300, 5000);
/* 4 */ fprintf(fpi,"4\n");
	movecart(pf, 300, 1000);
/* 5 */ fprintf(fpi,"5\n");
	movejnts(pm, 300, 1000);
/* 6 */ fprintf(fpi,"6\n");
	movejnts(pf, 300, 1000);
/* 7 */ fprintf(fpi,"7\n");
	movejnts(pm, 300, 1000);
/* 8 */ fprintf(fpi,"8\n");
	movejnts(pm, 300, 1000);
/* 9 */ fprintf(fpi,"9\n");
	movecart(pm, 300, 5000);
/* 10 */fprintf(fpi,"10\n");
	movejnts(pf, 300, 1000);
/* 11 */fprintf(fpi,"11\n");
	movecart(pm, 300, 5000);
/* 12 */fprintf(fpi,"12\n");
	movejnts(pm, 300, 1000);
/* 13 */fprintf(fpi,"13\n");
	movecart(park, 300, 2000);
}



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

	double omega = .2;
	double radius = 200.;

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