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

extern struct how how;
extern struct chg chg;
int sensor;

pumatask()
{
	TRSF_PTR z, b1, b2,fing, fins, over;
	POS_PTR  p1, p2, get;
	int fingfn();
	int q;

	fing = newtrans("FING",fingfn);
	Rot(fing, zunit, -90.);
	fins = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
	b1 = gentr_rot("B1", 600. ,-300., 450., yunit, 180.);
	b2 = gentr_rot("B2", 600. , 300., 450., yunit, 180.);
	over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);

	b1->fn = hold;
	p1 = makeposition("P1" , z, t6, fins, EQ, b1, TL, fins);
	p2 = makeposition("P1" , z, t6, fing, EQ, b2, TL, fing);
	get = makeposition("GET", z, t6, EQ, over, TL, t6);


	setvel(400, 300);
	move(get);
	waitfor(completed);
	OPEN
	printf("put the sensor ");
	QUERY(q);
	CLOSE
	printf("go ahead ");
	QUERY(q);
	if (q == 'n') {
		move(park);
		return;
	}
	sensor = adcopen(7);
	for (; ; ) {
		setmod('c');
		move(p1);
		stop(0);
		sample(15);
		movecart(p2, 200, 8000);
		sample(30);
		waitfor(completed);
		QUERY(q); if (q == 'n') break;
		move(get);
	}
	OPEN
	setmod('j');
	move(park);
}

#define CV      .010

fingfn(t)
TRSF_PTR t;
{
	t->p.z += (how.adcr[sensor] * CV - 3.) / 3.;
}
