home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
471
/
rccl248
< prev
next >
Wrap
Text File
|
1987-03-02
|
3KB
|
167 lines
#include "../h/rccl.h"
#include "../h/rtc.h"
#include "../h/which.h"
#include "../h/kine.h"
#include "../h/umac.h"
extern struct how how;
extern struct chg chg;
int idx = 0;
char outc;
#define MAX 1000
double cur1[MAX];
double cur2[MAX];
double cur3[MAX];
double cur4[MAX];
double cur5[MAX];
double cur6[MAX];
double tor1[MAX];
double tor2[MAX];
double tor3[MAX];
double tor4[MAX];
double tor5[MAX];
double tor6[MAX];
char cccc[MAX];
pumatask()
{
TRSF_PTR z, e, beg, end;
POS_PTR p1, p2;
int i;
int fc1, fc2, fc3, fc4, fc5, fc6;
int ft1, ft2, ft3, ft4, ft5, ft6;
int fcc;
int record();
e = trsl(newtrans("E", const), 0., 0., 170.);
z = gentr_rot("Z", 0., 0., 864., zunit, 0.); /* at the base */
beg = newtrans("BEG", varb);
end = newtrans("END", varb);
p1 = makeposition("P2", z, t6, e, EQ ,beg, TL, e);
p2 = makeposition("P3", z, t6, e, EQ, end, TL, e);
setvel(700, 700);
setmod('j');
if (!teach(beg, p1)) {
setvel(700, 700);
setmod('j');
move(park);
return;
}
if (!teach(end, p2)) {
setvel(700, 700);
setmod('j');
move(park);
return;
}
setmod('c');
move(p2);
setvel(70, 20);
outc = '-';
evalfn(record);
move(p1);
waitfor(completed);
outc = '+';
evalfn(record);
move(p2);
waitfor(completed);
setvel(700, 700);
move(park);
stop(100);
waitfor(completed);
release("happy");
fcc = creat("../g/c.out", 0644);
write(fcc, cccc, idx);
close(fcc);
ft1 = creat("../g/tor1.out", 0644);
ft2 = creat("../g/tor2.out", 0644);
ft3 = creat("../g/tor3.out", 0644);
ft4 = creat("../g/tor4.out", 0644);
ft5 = creat("../g/tor5.out", 0644);
ft6 = creat("../g/tor6.out", 0644);
write(ft1, tor1, idx * sizeof(double));
write(ft2, tor2, idx * sizeof(double));
write(ft3, tor3, idx * sizeof(double));
write(ft4, tor4, idx * sizeof(double));
write(ft5, tor5, idx * sizeof(double));
write(ft6, tor6, idx * sizeof(double));
close(ft1);
close(ft2);
close(ft3);
close(ft4);
close(ft5);
close(ft6);
fc1 = creat("../g/cur1.out", 0644);
fc2 = creat("../g/cur2.out", 0644);
fc3 = creat("../g/cur3.out", 0644);
fc4 = creat("../g/cur4.out", 0644);
fc5 = creat("../g/cur5.out", 0644);
fc6 = creat("../g/cur6.out", 0644);
write(fc1, cur1, idx * sizeof(double));
write(fc2, cur2, idx * sizeof(double));
write(fc3, cur3, idx * sizeof(double));
write(fc4, cur4, idx * sizeof(double));
write(fc5, cur5, idx * sizeof(double));
write(fc6, cur6, idx * sizeof(double));
exit(0);
}
record()
{
extern struct how how;
static unsigned short old[6];
double tor[6];
JNTS lod;
int i;
if (goalpos->scal < .2 || goalpos->scal > .9) {
for (i = 0; i < 6; ++i) {
old[i] = how.pos[i];
}
return;
}
if (idx >= MAX) {
return;
}
cur1[idx] = how.adcr[0];
cur2[idx] = how.adcr[1];
cur3[idx] = how.adcr[2];
cur4[idx] = how.adcr[3];
cur5[idx] = how.adcr[4];
cur6[idx] = how.adcr[5];
adctotor(tor, how.adcr, how.pos, old);
gravload(&lod,
sncs.c2, sncs.c23, sncs.s23, sncs.c4, sncs.s4, sncs.c5, sncs.s5);
tor1[idx] = tor[0] - lod.th1;
tor2[idx] = tor[1] - lod.th2;
tor3[idx] = tor[2] - lod.th3;
tor4[idx] = tor[3] - lod.th4;
tor5[idx] = tor[4] - lod.th5;
tor6[idx] = tor[5] - lod.th6;
cccc[idx] = outc;
++idx;
for (i = 0; i < 6; ++i) {
old[i] = how.pos[i];
}
}