home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL041 < prev    next >
Encoding:
Text File  |  1987-03-02  |  10.0 KB  |  446 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : equat.c
  7.  *      Remarks : Implements the equation structures.
  8.  *                The chosen ring structure does not make me quite happy.
  9.  *      Usage   : part of the library
  10.  */
  11.  
  12. /*LINTLIBRARY*/
  13.  
  14. #include "../h/rccl.h"
  15. #include "../h/manip.h"
  16.  
  17. /*
  18.  * sets up the data struct. representing a transform equation
  19.  * pt = makeposition(name, &L1, ..., &Ln, EQ, &R1, ..., &Rn, TL, toolptr);
  20.  */
  21.  
  22. #define ISCONS(t)       ((t)->trsf->fn == const)
  23. #define ISHOLD(t)       ((t)->trsf->fn == hold)
  24. #define ISVARB(t)       ((t)->trsf->fn == varb)
  25. #define ISFUND(t)       (!(ISCONS(t) || ISHOLD(t) || ISVARB(t)))
  26.  
  27. /* VARARGS */
  28. POS_PTR makeposition(sp, a)  /*::*/
  29. char *sp;
  30. TRSF_PTR a;
  31. {
  32.     int n2;                 /* counter                              */
  33.     char altname[80];       /* some storage for string management   */
  34.     TRSF_PTR mark;          /* for searching in the arg list        */
  35.     TRSF_PTR tp;            /* tool transform pointer               */
  36.     TRSF_PTR *pa;           /* pointer  to arg list in the stack    */
  37.     PST_PTR k;              /* what's being returned                */
  38.     TERM_PTR n, m, p, s, f; /* aux pointers to walk around          */
  39.     TERM_PTR newterm_n();   /* dyn. alloc                           */
  40.     PST_PTR newposition_n();  /* ........                           */
  41.     char *strcpy(),         /* those cause trouble for lint         */
  42.          *strcat(),         /* because bug in llib-lc               */
  43.          *strsave();
  44.  
  45.     pa = &a;                /* pa points on the first arg.          */
  46.     /*
  47.      * search for tp and check arg list
  48.      */
  49.     for(mark = (TRSF_PTR)EQ, n2 = 2; n2--; mark = (TRSF_PTR)TL) {
  50.         for (; *pa != mark; pa++) {
  51.             if (*pa == NULL) {
  52.                 fprintf(stderr, "position \"%s\" :", sp);
  53.                 giveup(
  54.             "transform not initialized - makeposition", YES);
  55.             }
  56.         }
  57.         pa++;           /* skip mark */
  58.     }
  59.  
  60.     tp = *pa;               /* toolptr */
  61.  
  62.     pa = &a;                /* reset pa */
  63.  
  64.     k = newposition_n();    /* create a position record     */
  65.     k->t6ptr = NULL;        /* for later checking           */
  66.     k->tlptr = NULL;        /* for later checking           */
  67.     k->name = sp;           /* pointer to string            */
  68.  
  69.     n = m = f = p = s = newterm_n();   /* create first term */
  70.     p->oldt = p->altr = p->trsf = (*pa);    /* first arg    */
  71.     p->rhs = NO;                            /* in lhs       */
  72.     if (p->trsf == t6) k->t6ptr = p;        /* set t6ptr if t6 met */
  73.     if (n->trsf == tp) k->tlptr = n;        /* set tlptr if tool met */
  74.  
  75.     while (*++pa != (TRSF_PTR)EQ) {   /* connect terms in given order */
  76.         n = newterm_n();
  77.         p->next = n;
  78.         n->prev = p;
  79.         n->oldt = n->altr = n->trsf = (*pa);
  80.         n->rhs = NO;
  81.         if (n->trsf == t6) k->t6ptr = n; /* set t6ptr if t6 met */
  82.         if (n->trsf == tp) k->tlptr = n; /* set tlptr if tool met */
  83.         p = n;              /* progress */
  84.     }
  85.  
  86.     while (*++pa != (TRSF_PTR)TL) { /* reverse connect */
  87.         m = newterm_n();
  88.         s->prev = m;
  89.         m->next = s;
  90.         m->oldt = m->altr = m->trsf = (*pa);
  91.         m->rhs = YES;
  92.         if (m->trsf == tp) k->tlptr = m;/* set tlptr if tool met */
  93.         s = m;            /* progress */
  94.     }
  95.  
  96.     n->next = m; /* finish the ring */
  97.     m->prev = n;
  98.  
  99.  
  100.     /*
  101.      * check if t6ptr and tlptr set
  102.      */
  103.  
  104.     if (k->t6ptr == NULL || k->tlptr == NULL) {
  105.         fprintf(stderr, "position \"%s\" :", k->name);
  106.         giveup("missing t6 or tool -  makeposition", YES);
  107.     }
  108.     if (f == m) {
  109.         fprintf(stderr, "position \"%s\" :", k->name);
  110.         giveup("missing rhs -  makeposition", YES);
  111.     }
  112.  
  113.     /*
  114.      * set the pos field
  115.      */
  116.  
  117.     k->pos = k->tlptr->next;
  118.     if (ISFUND(k->pos)) {
  119.         fprintf(stderr, "position \"%s\", transform \"%s\" :",
  120.             k->name, k->pos->trsf->name);
  121.         giveup("pos functionally defined - makeposition", YES);
  122.     }
  123.     if (k->pos == k->t6ptr) {
  124.         fprintf(stderr, "position \"%s\", :", k->name);
  125.         giveup("pos cannot seriously be t6 - makeposition", YES);
  126.     }
  127.  
  128.     /*
  129.      * print equation
  130.      */
  131.  
  132.     if (prints_out) {
  133.         fprintf(fpi, "makeposition, pos\t\"%s\"\t", k->name);
  134.         for (s = f; !s->rhs; s = s->next) {
  135.             fprintf(fpi, "%s ", s->trsf->name);
  136.             if (s->next == f) {
  137.                 break;
  138.             }
  139.         }
  140.         fprintf(fpi, " = ");
  141.         for (s = f->prev; s->rhs; s = s->prev) {
  142.             fprintf(fpi, "%s ", s->trsf->name);
  143.         }
  144.         fprintf(fpi, "\n");
  145.     }
  146.  
  147.     /*
  148.      * optimize
  149.      */
  150.  
  151.     optimize(k);
  152.  
  153.     /*
  154.      * allocate storage for the hold feature
  155.      */
  156.  
  157.     for (n = k->t6ptr->prev; n != k->t6ptr; n = n->prev) {
  158.         if (ISHOLD(n)) {
  159.             n->altr = newtrans(strsave(strcat(strcpy(altname,
  160.                     n->trsf->name), "(a)")), hold);
  161.             Assigntr(n->altr,  n->trsf);
  162.             n->oldt = newtrans(strsave(strcat(strcpy(altname,
  163.                     n->trsf->name), "(o)")), hold);
  164.             Assigntr(n->oldt,  n->trsf);
  165.         }
  166.     }
  167.  
  168.  
  169.     /*
  170.      * set the coord , tool, functional coord, functional tool predicates
  171.      * and print the canonized equation
  172.      */
  173.  
  174.     k->cfnsp = k->tfnsp = k->coorp = k->toolp = NO;
  175.     if (prints_out) {
  176.         fprintf(fpi,"\t\"COORD\":");
  177.     }
  178.     for (n = k->t6ptr->prev; n != k->pos; n = n->prev) {
  179.         k->coorp = YES;
  180.         if (ISFUND(n)) {
  181.             k->cfnsp = YES;
  182.         }
  183.         if (prints_out) {
  184.             fprintf(fpi,
  185.                 n->rhs ? " %s%s" : " -%s%s", n->trsf->name,
  186.     ISCONS(n) ? "" : ISHOLD(n) ? "(h)" : ISVARB(n) ? "(v)" : "(s)");
  187.         }
  188.     }
  189.     if (prints_out) {
  190.         fprintf(fpi,"  \"TOOL\":");
  191.     }
  192.     for (n = k->pos->prev; n != k->t6ptr; n = n->prev) {
  193.         k->toolp = YES;
  194.         if (ISFUND(n)) {
  195.             k->tfnsp = YES;
  196.         }
  197.         if (prints_out) {
  198.             fprintf(fpi,
  199.                 n->rhs ? " %s%s" : " -%s%s", n->trsf->name,
  200.     ISCONS(n) ? "" : ISHOLD(n) ? "(h)" : ISVARB(n) ? "(v)" : "(s)");
  201.         }
  202.     }
  203.     if (prints_out) {
  204.         fprintf(fpi,
  205.         k->pos->rhs ? "  \"POS\": %s%s\n\n" : "  \"POS\": -%s%s\n\n",
  206.             k->pos->trsf->name,
  207.             ISVARB(k->pos) ? "(v)" : ISHOLD(k->pos) ? "(h)" : "");
  208.     }
  209.     return((POS_PTR)k);
  210. }
  211.  
  212.  
  213. /*
  214.  * free an equation
  215.  */
  216.  
  217. freepos(p) /*::*/
  218. POS_PTR p;
  219. {
  220.     TERM_PTR t;
  221.  
  222.     for (t = ((PST_PTR)p)->t6ptr->prev;
  223.          t != ((PST_PTR)p)->t6ptr;
  224.          t = t->prev) {
  225.         if (ISHOLD(t)) {
  226.             free(t->altr->name);
  227.             freetrans(t->altr);
  228.             free(t->oldt->name);
  229.             freetrans(t->oldt);
  230.         }
  231.         if (t->trsf->name[0] == '_') {
  232.             free(t->trsf->name);
  233.             freetrans(t->trsf);
  234.         }
  235.         free((char *)t);
  236.     }
  237.     free((char *)t);
  238.     free((char *)p);
  239. }
  240.  
  241.  
  242. /*
  243.  * solve differential coordinate transforms
  244.  * #defining DEBUG helped
  245.  * those functions make sure that user's functions are called only once
  246.  * per sample time
  247.  * They are pretty shaky because they assume canonized equations
  248.  * where everything is in the rhs expect t6
  249.  * also they do not keep the inverses once calculated
  250.  * however they do not have to choose the best path, which is dictated
  251.  * format is : solve..(result, term1, term2)
  252.  */
  253.  
  254. #ifdef  DEBUG
  255. #define PREAMB  printf("%s %d %s : ",goalpos->name, rtime, r->name);
  256. #define POSTAMB printf("\n");
  257. #else
  258. #define PREAMB
  259. #define POSTAMB
  260. #endif
  261.  
  262. /*
  263.  * solve the inverse when in rhs, direct in lhs
  264.  */
  265.  
  266. #ifdef  DEBUG
  267. #define CTRF(t) (printf((t->rhs)?" -%s":" %s",t->altr->name),t->altr)
  268. #else
  269. #define CTRF(t) t->altr
  270. #endif
  271.  
  272. solvei_n(r, v, w) /*::*/
  273. TRSF_PTR r;
  274. TERM_PTR v, w;
  275. {
  276.     PREAMB
  277.     w = w->next;
  278.     if (w->trsf->timeval != rtime) {
  279.         (* w->trsf->fn)(w->trsf);
  280.         w->trsf->timeval = rtime;
  281.     }
  282.     if (w->rhs) {
  283.         Invert(r, CTRF(w));
  284.     }
  285.     else {
  286.         Assigntr(r, CTRF(w));
  287.     }
  288.     for (w = w->next; w != v; w = w->next) {
  289.         if (w->trsf->timeval != rtime) {
  290.             (* w->trsf->fn)(w->trsf);
  291.             w->trsf->timeval = rtime;
  292.         }
  293.         if (w->rhs) {
  294.             Trmultinv(r, CTRF(w));
  295.         }
  296.         else {
  297.             Trmultinp(r, CTRF(w));
  298.         }
  299.     }
  300.     POSTAMB
  301. }
  302.  
  303.  
  304. /*
  305.  * solve the direct when in rhs, inverse in lhs
  306.  */
  307.  
  308. #undef  CTRF
  309. #ifdef  DEBUG
  310. #define CTRF(t) (printf((t->rhs)?" %s":" -%s",t->altr->name),t->altr)
  311. #else
  312. #define CTRF(t) t->altr
  313. #endif
  314.  
  315. solved_n(r, v, w) /*::*/
  316. TRSF_PTR r;
  317. TERM_PTR v, w;
  318. {
  319.     PREAMB
  320.     v = v->prev;
  321.     if (v->trsf->timeval != rtime) {
  322.         (* v->trsf->fn)(v->trsf);
  323.         v->trsf->timeval = rtime;
  324.     }
  325.     if (v->rhs) {
  326.         Assigntr(r, CTRF(v));
  327.     }
  328.     else {
  329.         Invert(r, CTRF(v));
  330.     }
  331.     for (v = v->prev; v != w; v = v->prev) {
  332.         if (v->trsf->timeval != rtime) {
  333.             (* v->trsf->fn)(v->trsf);
  334.             v->trsf->timeval = rtime;
  335.         }
  336.         if (v->rhs) {
  337.             Trmultinp(r, CTRF(v));
  338.         }
  339.         else {
  340.             Trmultinv(r, CTRF(v));
  341.         }
  342.     }
  343.     POSTAMB
  344. }
  345.  
  346.  
  347.  
  348. /*
  349.  * same thing, but use 'old' transforms for calculating transitions
  350.  * with 'hold' transforms which may have changed meanhile
  351.  */
  352.  
  353. #undef  CTRF
  354. #ifdef  DEBUG
  355. #define CTRF(t) (printf((t->rhs)?" -%s":" %s",t->oldt->name),t->oldt)
  356. #else
  357. #define CTRF(t) t->oldt
  358. #endif
  359.  
  360. solveio_n(r, v, w) /*::*/
  361. TRSF_PTR r;
  362. TERM_PTR v, w;
  363. {
  364.     PREAMB
  365.     w = w->next;
  366.     if (w->trsf->timeval != rtime) {
  367.         (* w->trsf->fn)(w->trsf);
  368.         w->trsf->timeval = rtime;
  369.     }
  370.     if (w->rhs) {
  371.         Invert(r, CTRF(w));
  372.     }
  373.     else {
  374.         Assigntr(r, CTRF(w));
  375.     }
  376.     for (w = w->next; w != v; w = w->next) {
  377.         if (w->trsf->timeval != rtime) {
  378.             (* w->trsf->fn)(w->trsf);
  379.             w->trsf->timeval = rtime;
  380.         }
  381.         if (w->rhs) {
  382.             Trmultinv(r, CTRF(w));
  383.         }
  384.         else {
  385.             Trmultinp(r, CTRF(w));
  386.         }
  387.     }
  388.     POSTAMB
  389. }
  390.  
  391.  
  392. #undef  CTRF
  393. #ifdef  DEBUG
  394. #define CTRF(t) (printf((t->rhs)?" %s":" -%s",t->oldt->name),t->oldt)
  395. #else
  396. #define CTRF(t) t->oldt
  397. #endif
  398.  
  399. solvedo_n(r, v, w) /*::*/
  400. TRSF_PTR r;
  401. TERM_PTR v, w;
  402. {
  403.     PREAMB
  404.     v = v->prev;
  405.     if (v->trsf->timeval != rtime) {
  406.         (* v->trsf->fn)(v->trsf);
  407.         v->trsf->timeval = rtime;
  408.     }
  409.     if (v->rhs) {
  410.         Assigntr(r, CTRF(v));
  411.     }
  412.     else {
  413.         Invert(r, CTRF(v));
  414.     }
  415.     for (v = v->prev; v != w; v = v->prev) {
  416.         if (v->trsf->timeval != rtime) {
  417.             (* v->trsf->fn)(v->trsf);
  418.             v->trsf->timeval = rtime;
  419.         }
  420.         if (v->rhs) {
  421.             Trmultinp(r, CTRF(v));
  422.         }
  423.         else {
  424.             Trmultinv(r, CTRF(v));
  425.         }
  426.     }
  427.     POSTAMB
  428. }
  429.  
  430.  
  431. /*
  432.  * shift the old values of the hold transforms
  433.  */
  434.  
  435. shifttr_n(p) /*::*/
  436. PST_PTR p;
  437. {
  438.     register TERM_PTR t;
  439.  
  440.     for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
  441.         if (ISHOLD(t)) {
  442.             Assigntr(t->oldt, t->altr);
  443.         }
  444.     }
  445. }
  446.