home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl038
< prev
next >
Wrap
Text File
|
1987-03-02
|
18KB
|
892 lines
/*
* RCCL Version 1.0 Author : Vincent Hayward
* School of Electrical Engineering
* Purdue University
* Dir : src
* File : moves.c
* Remarks : This file contains the functions that make the
* the interface between the user and setpoint.
* Usage : part of the lib
*/
/*LINTLIBRARY*/
#include "../h/rccl.h"
#include "../h/manip.h"
/*
* static motion record that is set by the user copied into the queue
* and reset for each move request
*/
ROOT mqueue_n = {NULL, NULL}; /* queue header record */
MOT motionreq_n = {
0, 0, /* acct, sgt */
DEF_TRSV, DEF_ROTV, /* velt, velr */
NULL, /* pst */
'j', /* mod */
"", /* cfg */
NULL, /* upd */
NULL, /* fnt */
0, /* smpl */
0., /* fmass */
0, /* fsp */
0., 0., 0., 0., 0., 0., /* frc */
0, /* csp */
0., 0., 0., 0., 0., 0., /* cpy */
0, /* dsp */
0., 0., 0., 0., 0., 0., /* dst */
0, /* exp */
0., 0., 0., 0., 0., 0. /* exd */
};
static TRSF_PTR updtr = NULL; /* the transorm to update */
static PST_PTR updpos = NULL; /* in this pos equation */
/*
* the following functions merely set the different fields of the
* motion record.
* replacing them by macros would reduce the umber of entries in the
* library for the benefit of the loader but would make
* interlanguage communication impossible
*/
setmod(m) /*::*/
int m;
{
motionreq_n.mod = m;
}
setime(ta, ts) /*::*/
int ta, ts;
{
motionreq_n.acct = ta;
motionreq_n.sgt = ts;
}
setvel(t, r) /*::*/
int t, r;
{
motionreq_n.velt = t;
motionreq_n.velr = r;
}
evalfn(fn) /*::*/
TRFN fn;
{
motionreq_n.fnt = fn;
}
setconf(s) /*::*/
char *s;
{
motionreq_n.cfg = s;
}
sample(m) /*::*/
int m;
{
if (m < MINSAMPLE) {
m = MINSAMPLE;
}
if (m > MAXSAMPLE) {
m = MAXSAMPLE;
}
motionreq_n.smpl = m - m % HARDCLOCK;
}
massis(m) /*::*/
real m;
{
motionreq_n.fmass = m * 9.81;
}
/*
* this one is a special case, only a TERM_PTR need to be transmitted
* to the setpoint for solving, the corresponding term is set in movereq
*/
update(u, p) /*::*/
TRSF_PTR u;
POS_PTR p;
{
updtr = u;
updpos = (PST_PTR)p;
}
#define NmTONmm 1000.
#define DegTORad 0.01745329251994330
/*
* parses the argument string, and set the motion record limit entries
*/
/* VARARGS */
limit(c, a) /*::*/
char *c;
double a;
{
double *pa = &a;
while (*c) {
switch (*c++) {
case 'f' :
switch (*c++) {
case 'x' :
motionreq_n.fsp |= SELFX;
motionreq_n.frc.f.x = *pa++;
break;
case 'y' :
motionreq_n.fsp |= SELFY;
motionreq_n.frc.f.y = *pa++;
break;
case 'z' :
motionreq_n.fsp |= SELFZ;
motionreq_n.frc.f.z = *pa++;
break;
default :
goto bad;
}
break;
case 't' :
switch (*c++) {
case 'x' :
motionreq_n.fsp |= SELMX;
motionreq_n.frc.m.x = *pa++ * NmTONmm;
break;
case 'y' :
motionreq_n.fsp |= SELMY;
motionreq_n.frc.m.y = *pa++ * NmTONmm;
break;
case 'z' :
motionreq_n.fsp |= SELMZ;
motionreq_n.frc.m.z = *pa++ * NmTONmm;
break;
default :
goto bad;
}
break;
case 'd' :
switch (*c++) {
case 'x' :
motionreq_n.dsp |= SELTX;
motionreq_n.dst.t.x = *pa++;
break;
case 'y' :
motionreq_n.dsp |= SELTY;
motionreq_n.dst.t.y = *pa++;
break;
case 'z' :
motionreq_n.dsp |= SELTZ;
motionreq_n.dst.t.z = *pa++;
break;
default :
goto bad;
}
break;
case 'r' :
switch (*c++) {
case 'x' :
motionreq_n.dsp |= SELRX;
motionreq_n.dst.r.x = *pa++ * DegTORad;
break;
case 'y' :
motionreq_n.dsp |= SELRY;
motionreq_n.dst.r.y = *pa++ * DegTORad;
break;
case 'z' :
motionreq_n.dsp |= SELRZ;
motionreq_n.dst.r.z = *pa++ * DegTORad;
break;
default :
goto bad;
}
break;
case ' ' :
break;
default :
goto bad;
}
}
return;
bad :
giveup("bad spec. - limit", YES);
}
/*
* comply and lock parse the string argument and the set the comply entry
* perform set operations to make sure specs are consistent
*/
/* VARARGS */
comply(c, a) /*::*/
char *c;
double a;
{
double *pa = &a;
while (*c) {
switch (*c++) {
case 'f' :
switch (*c++) {
case 'x' :
if (motionreq_n.csp & SELFX) {
goto bad;
}
motionreq_n.csp |= SELFX;
motionreq_n.cpy.f.x = *pa++;
break;
case 'y' :
if (motionreq_n.csp & SELFY) {
goto bad;
}
motionreq_n.csp |= SELFY;
motionreq_n.cpy.f.y = *pa++;
break;
case 'z' :
if (motionreq_n.csp & SELFZ) {
goto bad;
}
motionreq_n.csp |= SELFZ;
motionreq_n.cpy.f.z = *pa++;
break;
default :
goto bad;
}
break;
case 't' :
switch (*c++) {
case 'x' :
if (motionreq_n.csp & SELMX) {
goto bad;
}
motionreq_n.csp |= SELMX;
motionreq_n.cpy.m.x = *pa++ * NmTONmm;
break;
case 'y' :
if (motionreq_n.csp & SELMY) {
goto bad;
}
motionreq_n.csp |= SELMY;
motionreq_n.cpy.m.y = *pa++ * NmTONmm;
break;
case 'z' :
if (motionreq_n.csp & SELMZ) {
goto bad;
}
motionreq_n.csp |= SELMZ;
motionreq_n.cpy.m.z = *pa++ * NmTONmm;
break;
default :
goto bad;
}
break;
case ' ' :
break;
default :
goto bad;
}
}
return;
bad :
giveup("bad force spec. - comply", YES);
}
lock(c) /*::*/
char *c;
{
while (*c) {
switch (*c++) {
case 'f' :
switch (*c++) {
case 'x' :
if (!(motionreq_n.csp & SELFX)) {
goto bad;
}
motionreq_n.csp &= ~SELFX;
motionreq_n.cpy.f.x = 0.;
break;
case 'y' :
if (!(motionreq_n.csp & SELFY)) {
goto bad;
}
motionreq_n.csp &= ~SELFY;
motionreq_n.cpy.f.y = 0.;
break;
case 'z' :
if (!(motionreq_n.csp & SELFZ)) {
goto bad;
}
motionreq_n.csp &= ~SELFZ;
motionreq_n.cpy.f.z = 0.;
break;
default :
goto bad;
}
break;
case 't' :
switch (*c++) {
case 'x' :
if (!(motionreq_n.csp & SELMX)) {
goto bad;
}
motionreq_n.csp &= ~SELMX;
motionreq_n.cpy.m.x = 0.;
break;
case 'y' :
if (!(motionreq_n.csp & SELMY)) {
goto bad;
}
motionreq_n.csp &= ~SELMY;
motionreq_n.cpy.m.y = 0.;
break;
case 'z' :
if (!(motionreq_n.csp & SELMZ)) {
goto bad;
}
motionreq_n.csp &= ~SELMZ;
motionreq_n.cpy.m.z = 0.;
break;
default :
goto bad;
}
break;
case ' ' :
break;
default :
goto bad;
}
}
return;
bad :
giveup("bad force spec. - lock", YES);
}
/*
* same principle as above but for distances, no check necessary
*/
/* VARARGS */
distance(c, a) /*::*/
char *c;
double a;
{
double *pa = &a;
while (*c) {
switch (*c++) {
case 'd' :
switch (*c++) {
case 'x' :
motionreq_n.exp |= SELTX;
motionreq_n.exd.t.x = *pa++;
break;
case 'y' :
motionreq_n.exp |= SELTY;
motionreq_n.exd.t.y = *pa++;
break;
case 'z' :
motionreq_n.exp |= SELTZ;
motionreq_n.exd.t.z = *pa++;
break;
default :
goto bad;
}
break;
case 'r' :
switch (*c++) {
case 'x' :
motionreq_n.exp |= SELRX;
motionreq_n.exd.r.x = *pa++ * DegTORad;
break;
case 'y' :
motionreq_n.exp |= SELRY;
motionreq_n.exd.r.y = *pa++ * DegTORad;
break;
case 'z' :
motionreq_n.exp |= SELRZ;
motionreq_n.exd.r.z = *pa++ * DegTORad;
break;
default :
goto bad;
}
break;
case ' ' :
break;
default :
goto bad;
}
}
return;
bad :
giveup("bad distance spec. - distance", YES);
}
/*
* move, set up the position and issue the request
*/
move(p) /*::*/
POS_PTR p;
{
motionreq_n.pst = (PST_PTR)p;
movereq();
}
/*
* stop issue the request without pos, means redo the last one
*/
stop(t) /*::*/
int t;
{
motionreq_n.sgt = t;
movereq();
}
/*
* 1) make sure specs are consitent
* 2) print the requests
* 3) put them in the queue
* 4) reset the record
*/
movereq() /*::*/
{
static int sam = DEF_SAMPLE;
static FORCE zeroforce = {0., 0., 0., 0., 0., 0.};
static DIFF zerodiff = {0., 0., 0., 0., 0., 0.};
char *new();
if (*(motionreq_n.cfg) != '\0' && motionreq_n.mod != 'j') {
giveup("conf must change in joint mode", YES);
}
if (motionreq_n.mod != 'j' && motionreq_n.mod != 'c') {
motionreq_n.mod = 'j';
}
if (motionreq_n.smpl != 0) {
sam = motionreq_n.smpl;
}
motionreq_n.acct /= sam;
motionreq_n.sgt /= sam;
if (motionreq_n.sgt == 0) {
if (motionreq_n.velt <= 0) {
motionreq_n.velt = DEF_TRSV;
}
if (motionreq_n.velr <= 0) {
motionreq_n.velr = DEF_ROTV;
}
if (motionreq_n.acct == 0) {
motionreq_n.acct = 2 +
((motionreq_n.velt + motionreq_n.velr) / (20 * sam));
}
}
else {
if (motionreq_n.acct == 1) {
motionreq_n.acct = 2;
}
if (2 * motionreq_n.acct > motionreq_n.sgt) {
motionreq_n.sgt = 2 * motionreq_n.acct;
}
}
if (motionreq_n.pst != NULL) {
PST_PTR p = motionreq_n.pst;
TERM_PTR t;
TRSF_PTR h;
ITEM_PTR ih;
for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
if (t->trsf->fn == hold) {
if ((ih = (ITEM_PTR)new(sizeof(ITEM) +
sizeof(TRSF)))
== NULL) {
giveup("alloc err", YES);
}
h = (TRSF_PTR)((char *)ih + sizeof(ITEM));
Assigntr(h, t->trsf);
enqueue_n(&(t->hd), ih);
}
}
if (updtr != NULL) {
if (updtr->fn != varb) {
giveup("invalid update transform type", YES);
}
p = updpos;
for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
if (t->trsf == updtr) {
motionreq_n.upd = t;
break;
}
}
if (motionreq_n.upd == NULL) {
giveup("could'nt find updatable transform", YES);
}
}
}
if (prints_out) {
fprintf(fpi,
"request %s mode %c acct %d sgt %d velt %d velr %d\n",
(motionreq_n.pst) ? motionreq_n.pst->name : "STOP",
motionreq_n.mod,
motionreq_n.acct * sam,
motionreq_n.sgt * sam,
motionreq_n.velt,
motionreq_n.velr);
fprintf(fpi,
"conf %s %s upd : %s smpl %d mass %f\n",
motionreq_n.cfg,
(motionreq_n.fnt) ? "EVAL" : "",
(motionreq_n.upd) ? motionreq_n.upd->trsf->name : "",
motionreq_n.smpl,
motionreq_n.fmass);
if (motionreq_n.fsp & SELFX) {
fprintf(fpi, "stop fx : %-5g\n", motionreq_n.frc.f.x);
}
if (motionreq_n.fsp & SELFY) {
fprintf(fpi, "stop fy : %-5g\n", motionreq_n.frc.f.y);
}
if (motionreq_n.fsp & SELFZ) {
fprintf(fpi, "stop fz : %-5g\n", motionreq_n.frc.f.z);
}
if (motionreq_n.fsp & SELMX) {
fprintf(fpi, "stop mx : %-5g\n", motionreq_n.frc.m.x);
}
if (motionreq_n.fsp & SELMY) {
fprintf(fpi, "stop my : %-5g\n", motionreq_n.frc.m.y);
}
if (motionreq_n.fsp & SELMZ) {
fprintf(fpi, "stop mz : %-5g\n", motionreq_n.frc.m.z);
}
if (motionreq_n.csp & SELFX) {
fprintf(fpi, "cply fx : %-5g\n", motionreq_n.cpy.f.x);
}
if (motionreq_n.csp & SELFY) {
fprintf(fpi, "cply fy : %-5g\n", motionreq_n.cpy.f.y);
}
if (motionreq_n.csp & SELFZ) {
fprintf(fpi, "cply fz : %-5g\n", motionreq_n.cpy.f.z);
}
if (motionreq_n.csp & SELMX) {
fprintf(fpi, "cply mx : %-5g\n", motionreq_n.cpy.m.x);
}
if (motionreq_n.csp & SELMY) {
fprintf(fpi, "cply my : %-5g\n", motionreq_n.cpy.m.y);
}
if (motionreq_n.csp & SELMZ) {
fprintf(fpi, "cply mz : %-5g\n", motionreq_n.cpy.m.z);
}
if (motionreq_n.dsp & SELTX) {
fprintf(fpi, "stop dx : %-5g\n", motionreq_n.dst.t.x);
}
if (motionreq_n.dsp & SELTY) {
fprintf(fpi, "stop dy : %-5g\n", motionreq_n.dst.t.y);
}
if (motionreq_n.dsp & SELTZ) {
fprintf(fpi, "stop dz : %-5g\n", motionreq_n.dst.t.z);
}
if (motionreq_n.dsp & SELRX) {
fprintf(fpi, "stop rx : %-5g\n", motionreq_n.dst.r.x);
}
if (motionreq_n.dsp & SELRY) {
fprintf(fpi, "stop ry : %-5g\n", motionreq_n.dst.r.y);
}
if (motionreq_n.dsp & SELRZ) {
fprintf(fpi, "stop rz : %-5g\n", motionreq_n.dst.r.z);
}
if (motionreq_n.exp & SELTX) {
fprintf(fpi, "dist dx : %-5g\n", motionreq_n.exd.t.x);
}
if (motionreq_n.exp & SELTY) {
fprintf(fpi, "dist dy : %-5g\n", motionreq_n.exd.t.y);
}
if (motionreq_n.exp & SELTZ) {
fprintf(fpi, "dist dz : %-5g\n", motionreq_n.exd.t.z);
}
if (motionreq_n.exp & SELRX) {
fprintf(fpi, "dist rx : %-5g\n", motionreq_n.exd.r.x);
}
if (motionreq_n.exp & SELRY) {
fprintf(fpi, "dist ry : %-5g\n", motionreq_n.exd.r.y);
}
if (motionreq_n.exp & SELRZ) {
fprintf(fpi, "dist rz : %-5g\n", motionreq_n.exd.r.z);
}
fprintf(fpi, "\n");
}
++requestnb;
{
ITEM_PTR im;
MOT_PTR m;
if ((im = (ITEM_PTR)new(sizeof(ITEM) + sizeof(MOT))) == NULL) {
giveup("alloc err", YES);
}
m = (MOT_PTR)((char *)im + sizeof(ITEM));
m->acct = motionreq_n.acct;
m->sgt = motionreq_n.sgt;
m->velt = motionreq_n.velt;
m->velr = motionreq_n.velr;
m->pst = motionreq_n.pst;
m->mod = motionreq_n.mod;
m->cfg = motionreq_n.cfg;
m->fnt = motionreq_n.fnt;
m->upd = motionreq_n.upd;
m->smpl = motionreq_n.smpl;
m->fmass = motionreq_n.fmass;
m->fsp = motionreq_n.fsp;
Assignforce(&m->frc, &motionreq_n.frc);
m->csp = motionreq_n.csp;
Assignforce(&m->cpy, &motionreq_n.cpy);
m->dsp = motionreq_n.dsp;
Assigndiff(&m->dst, &motionreq_n.dst);
m->exp = motionreq_n.exp;
Assigndiff(&m->exd, &motionreq_n.exd);
enqueue_n(&mqueue_n, im);
}
motionreq_n.acct = 0;
motionreq_n.sgt = 0;
/* no velocities reset */
motionreq_n.pst = NULL;
motionreq_n.cfg = "";
motionreq_n.upd = NULL;
motionreq_n.fnt = NULL;
updtr = NULL;
updpos = NULL;
motionreq_n.smpl = 0;
/* no mass reset */
motionreq_n.fsp = 0;
Assignforce(&motionreq_n.frc, &zeroforce);
/* no comply stuff reset */
motionreq_n.dsp = 0;
Assigndiff(&motionreq_n.dst, &zerodiff);
motionreq_n.exp = 0;
Assigndiff(&motionreq_n.exd, &zerodiff);
}
/*
* queue and dequeue motion requests
* queues have a header with two pointers put and get and
* are a singly linked list with one pointer and a data field
* items of any size can be handled
* enqueue uses a pointer to a preallocated area where the data is stored
* dequeue returns a pointer to the data and free the allocated area
*/
static int enqbusy = NO, deqbusy = NO;
enqueue_n(r, l) /*:: put a record in a queue */
register ROOT_PTR r;
register ITEM_PTR l;
{
enqbusy = YES;
while (deqbusy) {
fprintf(stderr, "*** could'nt queue at %d\n", rtime);
nap(5);
/*NOTREACHED*/
}
if (r->put == NULL) {
r->get = l;
}
else {
r->put->next = l;
}
r->put = l;
r->put->next = NULL;
enqbusy = NO;
}
ITEM_PTR dequeue_n(r) /*::*/
register ROOT_PTR r;
{
register ITEM_PTR g;
deqbusy = YES;
if (enqbusy) {
deqbusy = NO;
return(NULL); /* stop the arm */
}
if (r->get == NULL) {
deqbusy = NO;
return(NULL);
}
g = r->get;
r->get = r->get->next;
if (r->get == NULL) {
r->put = NULL;
}
disp((char *)g);
deqbusy = NO;
return((ITEM_PTR)((char *)g + sizeof(ITEM)));
}
/*
* storage allocator for the private use of queue/dequeue
* inspired from Kernighan
*/
#define NPOOL 4096
struct s {
union head *ptr;
unsigned size;
};
typedef union head {
struct s s;
ALIGN x;
} HEAD;
typedef struct pl {
struct s s;
HEAD st[NPOOL];
} POOL;
static POOL pool;
static HEAD *allocp = NULL;
static char *new(nb) /*##*/
int nb;
{
register HEAD *p, *q;
register int nunits = 1 + (nb + sizeof(HEAD) - 1) / sizeof(HEAD);
if ((q = allocp) == NULL) {
pool.s.ptr = allocp = q = (HEAD *)&pool;
pool.s.size = NPOOL;
}
for (p = q->s.ptr; ; q = p, p = p->s.ptr) {
if (p->s.size >= nunits) {
if (p->s.size == nunits) {
q->s.ptr = p->s.ptr;
}
else {
p->s.size -= nunits;
p += p->s.size;
p->s.size = nunits;
}
allocp = q;
return((char *)(p + 1));
}
if (p == allocp) {
return(NULL);
}
}
}
static disp(ap) /*##*/
register char *ap;
{
register HEAD *p, *q;
p = (HEAD *) ap - 1;
for (q = allocp; !(p > q && p < q->s.ptr); q = q->s.ptr)
if (q >= q->s.ptr && (p > q || p < q->s.ptr))
break;
if (p + p->s.size == q->s.ptr) {
p->s.size += q->s.ptr->s.size;
p->s.ptr = q->s.ptr->s.ptr;
}
else {
p->s.ptr = q->s.ptr;
}
if (q + q->s.size == p) {
q->s.size += p->s.size;
q->s.ptr = p->s.ptr;
}
else {
q->s.ptr = p;
}
allocp = q;
}