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

int turns;

pumatask()
{
	TRSF_PTR z, e, shaft, handle, apro, grasp, rotpx, rotnx;
	POS_PTR  get, away;
	POS_PTR turn;
	int pxfn(), nxfn();
	int q;

	rotpx = newtrans("ROTPX", pxfn);
	rotnx = newtrans("ROTNX", nxfn);
	z = gentr_trsl("Z",  0.,  0., 864.);
	e = gentr_trsl("E" , 0. , 0. , 170.);
	shaft = gentr_trsl("SHAFT", -200., 500., 600.);
	shaft->fn = varb;
	handle = gentr_trsl("HANDLE", 0., 0., 50.);
	apro = gentr_trsl("APRO", -50., 0., 0.);
	grasp = gentr_rpy("GRASP", 0., 0., 0., 0., 190., 0.);

	get = makeposition(
	"GET", z, t6, e, EQ, shaft, handle, grasp, TL, e);

	away = makeposition(
	"AWAY", z, t6, e, EQ, shaft, handle, grasp, apro, TL, e);

	turn = makeposition(
	"TURN", z, t6, e, EQ, shaft, rotpx, handle, rotnx, grasp, TL, rotnx);

	setvel(300, 300);
	move(away);
	OPEN;
	if (!teach(shaft, get)) {
		move(away);
		move(park);
		return;
	}
	shaft->fn = const;
	optimize(turn);
	turns = 4;
	waitfor(completed);
	CLOSE;
	comply("fx fz ", 0., 0.);
		movecart(turn, 200, 4000 * turns);
	lock("fx fz ");
	move(get);
	waitfor(turn->end);
	OPEN;
	distance("dx", -30.);
		move(get);
	setvel(200, 50);
	setmod('j');
	move(park);
}

pxfn(t)
TRSF_PTR t;
{
	Rot(t, xunit, goalpos->scal * 360 * turns);
}

nxfn(t)
TRSF_PTR t;
{
	Rot(t, xunit, - goalpos->scal * 360 * turns);
}
