Between Heaven & Hell 2/BetweenHeavenHell.cdr/500/470/rccl030

Text File | 1987-03-02 | 5KB | 215 lines

/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : other.c
 *      Remarks : The functions here are called by setpoint to deal with
 *                set of joint values or drive parameters.
 *      Usage   : part of the library
 */

#include "../h/rccl.h"
#include "../h/manip.h"

/*
 * perform joint structure value copy
 */

assignjs_n(d, s) /*::*/
register JNTS_PTR d, s;
{
    d->th1 = s->th1;
    d->th2 = s->th2;
    d->th3 = s->th3;
    d->th4 = s->th4;
    d->th5 = s->th5;
    d->th6 = s->th6;
}

/*
 * perform joint structure value difference
 */

diffjnts_n(d, x, y) /*::*/
register JNTS_PTR d, x, y;
{
    d->th1 = x->th1 - y->th1;
    d->th2 = x->th2 - y->th2;
    d->th3 = x->th3 - y->th3;
    d->th4 = x->th4 - y->th4;
    d->th5 = x->th5 - y->th5;
    d->th6 = x->th6 - y->th6;
}

/*
 * perform joint structure value first order polynomial y = a * x + b
 */

fojnts_n(y, a, b, x)      /*::*/
register JNTS_PTR y, a, b;
real x;
{
    y->th1 = a->th1 * x + b->th1;
    y->th2 = a->th2 * x + b->th2;
    y->th3 = a->th3 * x + b->th3;
    y->th4 = a->th4 * x + b->th4;
    y->th5 = a->th5 * x + b->th5;
    y->th6 = a->th6 * x + b->th6;
}


/*
 * perform joint structure value fast times -2
 */

t2jnts_n(y, x)  /*::*/
register JNTS_PTR y, x;
{
    y->th1 = - (x->th1 + x->th1);
    y->th2 = - (x->th2 + x->th2);
    y->th3 = - (x->th3 + x->th3);
    y->th4 = - (x->th4 + x->th4);
    y->th5 = - (x->th5 + x->th5);
    y->th6 = - (x->th6 + x->th6);
}


/*
 * straight part j mode polynomial adjustment in comply mode
 */

focpyc_n(jt, jf, x, jo, cpy)  /*::*/
register JNTS_PTR jt, jf;
real x;
register JNTS_PTR jo;
int cpy;
{
    if (cpy & SELJ1) {
        jt->th1 = jo->th1 - jf->th1 * x;
    }
    if (cpy & SELJ2) {
        jt->th2 = jo->th2 - jf->th2 * x;
    }
    if (cpy & SELJ3) {
        jt->th3 = jo->th3 - jf->th3 * x;
    }
    if (cpy & SELJ4) {
        jt->th4 = jo->th4 - jf->th4 * x;
    }
    if (cpy & SELJ5) {
        jt->th5 = jo->th5 - jf->th5 * x;
    }
    if (cpy & SELJ6) {
        jt->th6 = jo->th6 - jf->th6 * x;
    }
}


/*
 * quartic j mode transition
 */

polyjnts_n(r, c1, c2, c3, x, y) /*::*/
register JNTS_PTR r, c1, c2, c3;
real x, y;
{
    r->th1 = (c1->th1 * x + c2->th1) * y + c3->th1;
    r->th2 = (c1->th2 * x + c2->th2) * y + c3->th2;
    r->th3 = (c1->th3 * x + c2->th3) * y + c3->th3;
    r->th4 = (c1->th4 * x + c2->th4) * y + c3->th4;
    r->th5 = (c1->th5 * x + c2->th5) * y + c3->th5;
    r->th6 = (c1->th6 * x + c2->th6) * y + c3->th6;
}


/*
 * transitiom part j mode polynomial adjustment in comply mode
 */

polycpyc_n(jt, jf, jg, x, y, jo, cpy) /*::*/
register JNTS_PTR jt, jf, jg;
real x, y;
JNTS_PTR jo;
int cpy;
{
    if (cpy & SELJ1) {
        jt->th1 = jo->th1 - (jf->th1 * x + jg->th1) * y;
    }
    if (cpy & SELJ2) {
        jt->th2 = jo->th2 - (jf->th2 * x + jg->th2) * y;
    }
    if (cpy & SELJ3) {
        jt->th3 = jo->th3 - (jf->th3 * x + jg->th3) * y;
    }
    if (cpy & SELJ4) {
        jt->th4 = jo->th4 - (jf->th4 * x + jg->th4) * y;
    }
    if (cpy & SELJ5) {
        jt->th5 = jo->th5 - (jf->th5 * x + jg->th5) * y;
    }
    if (cpy & SELJ6) {
        jt->th6 = jo->th6 - (jf->th6 * x + jg->th6) * y;
    }
}


/*
 * first order polynomial for drive parameters  y = a * x + b (b can be NULL)
 */

fopar_n(y, a, b, x) /*::*/
register DRVP_PTR y, a, b;
real x;
{
    if (b == (DRVP_PTR)NULL) {
        y->dx = a->dx * x;
        y->dy = a->dy * x;
        y->dz = a->dz * x;
        y->dphi = a->dphi * x;
        y->dthe = a->dthe * x;
    }
    else {
        y->dx = a->dx * x + b->dx;
        y->dy = a->dy * x + b->dy;
        y->dz = a->dz * x + b->dz;
        y->dphi = a->dphi * x + b->dphi;
        y->dthe = a->dthe * x + b->dthe;
    }
    y->dpsi = a->dpsi;
}


/*
 * perform fast drive parameters times -2
 */

t2par_n(y, x) /*::*/
register DRVP_PTR y, x;
{
    y->dx = - (x->dx + x->dx);
    y->dy = - (x->dy + x->dy);
    y->dz = - (x->dz + x->dz);
    y->dphi = - (x->dphi + x->dphi);
    y->dthe = - (x->dthe + x->dthe);
}


/*
 * perform quartic transition of drive parameters
 * plus a third order polynomial for velocity adjustment
 */

polypar_n(r, c1, x, c2, y, c3, c4, z, c5, t) /*::*/
register DRVP_PTR r, c1, c2, c3, c4, c5;
real x, y, z, t;
{
    r->dx = (c1->dx * x + c2->dx) * y + c3->dx + c4->dx * z + c5->dx * t;
    r->dy = (c1->dy * x + c2->dy) * y + c3->dy + c4->dy * z + c5->dy * t;
    r->dz = (c1->dz * x + c2->dz) * y + c3->dz + c4->dz * z + c5->dz * t;
    r->dphi = (c1->dphi * x + c2->dphi) * y + c3->dphi + c4->dphi * z
                               + c5->dphi * t;
    r->dthe = (c1->dthe * x + c2->dthe) * y + c3->dthe + c4->dthe * z
                               + c5->dthe * t;
}