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

extern struct how how;
extern struct chg chg;

int sensor;

pumatask()
{
	TRSF_PTR z, e, b, ab, un;
	POS_PTR  p1, p2, p3;
	int i;

	e = trsl(newtrans("E", const), 0., 0., 170.);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	ab = gentr_trsl("AB", 0. , 0., -500.);
	un = gentr_trsl("AB", 0. , 0., -30.);
	b = gentr_rot("B1", 300. , 600., 300., yunit, 180.);
	b->fn = varb;


	p1 = makeposition("P1", z, t6, e, EQ, b, ab, TL, e);
	p2 = makeposition("P2", z, t6, e, EQ ,b, TL, e);
	p3 = makeposition("P3", z, t6, e, EQ, b, un, TL, e);

	setvel(700, 700);
	setmod('j');
	move(p1);
	if (!teach(b, p2)) {
		setvel(700, 700);
		setmod('j');
		move(park);
		return;
	}
	OPEN;
	setvel(500, 500);
	setmod('j');
	move(p3);
	move(park);
	for (i = 0; i < 4; ++i) {
		move(p3);
		move(p2);
		stop(300);
		waitfor(completed);
		stop(100);
		CLOSE;
		move(p3);
		move(park);
		move(p3);
		move(p2);
		stop(0);
		waitfor(completed);
		OPEN;
		move(p3);
		move(park);
	}
}
