home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
471
/
rccl199
< prev
next >
Wrap
Text File
|
1987-03-02
|
2KB
|
90 lines
#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, e, h, search, track, pt, over;
POS_PTR p1, p2, p3, get;
int searchfn(), trackfn();
int q;
search = newtrans("FING",searchfn);
track = newtrans("FING",trackfn);
h = newtrans("H", const);
z = gentr_rot("Z", 0., 0., 864., zunit, 0.);
e = gentr_eul("E" , 0. , 0. , 200. , 0. , 0., 0.);
b1 = gentr_rot("B1", 600. ,-400., 450., yunit, 180.);
pt = gentr_rot("PT", 0., 0., 0., zunit, 90.);
over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
p1 = makeposition("P1" , z, t6, e, EQ, b1, pt, TL, pt);
p2 = makeposition("P2" , z, t6, e, search, EQ, b1, pt, TL, pt);
p3 = makeposition("P3" , z, t6, e, h, track, EQ, b1, pt, TL, e);
get = makeposition("GET", z, t6, EQ, over, TL, t6);
movejnts(get, 400, 1500);
waitfor(completed);
OPEN
printf("put the sensor ");
QUERY(q);
CLOSE
printf("go ahead ");
QUERY(q);
if (q == 'n') {
movejnts(park, 100, 20000);
return;
}
sensor = adcopen(7);
movecart(p1, 400, 1500);
movecart(p2, 100, 20000);
movecart(p3, 100, 20000);
OPEN
movejnts(park, 400, 1500);
}
#define CV .010
#define STEP 1.
double ZT;
searchfn(t)
TRSF_PTR t;
{
double alpha = 0., inc;
int fl;
t->p.z -= STEP;
fl = how.adcr[sensor];
if ((fl * CV - 2.5) * .1 > 0.) {
nextmove = YES;
ZT = t->p.z;
}
}
trackfn(t)
TRSF_PTR t;
{
double alpha = 0., inc;
int fl;
static int first = YES;
if (first) {
first = NO;
t->p.z = ZT;
}
t->p.x -= STEP;
fl = how.adcr[sensor];
inc = (fl * CV - 2.5);
t->p.z += inc * .05;
alpha = atan2(inc , STEP);
alpha *= RADTODEG;
rotm(t, yunit, -alpha);
}