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

extern struct how how;

static int sensor;

#define TOUCHED 10

pumatask()
{
	int touchfn();
	TRSF_PTR z, b1, b2, fing, getit, flip;
	POS_PTR  get, p1, p2;
	int q;

	z = gentr_trsl("Z",  0.,  0., 864.);
	b1 = gentr_trsl("B1", 600. ,-200., 400.);
	b2 = gentr_trsl("B2", 600. ,-100., 400.);
	fing = gentr_rot("FING", 0., 0., 200., zunit, -90.);
	getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);
	flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);

	p1 = makeposition("P1" , z, t6, fing, EQ, b1, flip,TL , fing);
	p2 = makeposition("P2" , z, t6, fing, EQ, b2, flip,TL , fing);
	get = makeposition("GET", z, t6, EQ, getit, TL, t6);

	sensor = adcopen(7);
	setvel(300, 100);
	move(get);
	setime(100, 10000);
	evalfn(touchfn);
	move(get) ;
	waitfor(completed);
	if (get->code != TOUCHED) {
		printf("timeout\n");
	}
	move(park);
}


touchfn()
{
	if (how.adcr[sensor] > 5) {
		nextmove = TOUCHED;
	}
}
