home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL038 < prev    next >
Encoding:
Text File  |  1987-03-02  |  17.3 KB  |  892 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : moves.c
  7.  *      Remarks : This file contains the functions that make the
  8.  *                the interface between the user and setpoint.
  9.  *      Usage   : part of the lib
  10.  */
  11.  
  12. /*LINTLIBRARY*/
  13.  
  14. #include "../h/rccl.h"
  15. #include "../h/manip.h"
  16.  
  17. /*
  18.  * static motion record that is set by the user copied into the queue
  19.  * and reset for each move request
  20.  */
  21.  
  22. ROOT mqueue_n = {NULL, NULL};           /* queue header record  */
  23. MOT motionreq_n = {
  24.     0, 0,                           /* acct, sgt    */
  25.     DEF_TRSV, DEF_ROTV,             /* velt, velr   */
  26.     NULL,                           /* pst          */
  27.     'j',                            /* mod          */
  28.     "",                             /* cfg          */
  29.     NULL,                           /* upd          */
  30.     NULL,                           /* fnt          */
  31.     0,                              /* smpl         */
  32.     0.,                             /* fmass        */
  33.     0,                              /* fsp          */
  34.     0., 0., 0., 0., 0., 0.,         /* frc          */
  35.     0,                              /* csp          */
  36.     0., 0., 0., 0., 0., 0.,         /* cpy          */
  37.     0,                              /* dsp          */
  38.     0., 0., 0., 0., 0., 0.,         /* dst          */
  39.     0,                              /* exp          */
  40.     0., 0., 0., 0., 0., 0.          /* exd          */
  41. };
  42.  
  43. static TRSF_PTR updtr = NULL;           /* the transorm to update       */
  44. static PST_PTR updpos = NULL;           /* in this pos equation         */
  45.  
  46.  
  47. /*
  48.  * the following functions merely set the different fields of the
  49.  * motion record.
  50.  * replacing them by macros would reduce the umber of entries in the
  51.  * library for the benefit of the loader but would make
  52.  * interlanguage communication impossible
  53.  */
  54.  
  55.  
  56. setmod(m) /*::*/
  57. int m;
  58. {
  59.     motionreq_n.mod = m;
  60. }
  61.  
  62.  
  63. setime(ta, ts) /*::*/
  64. int ta, ts;
  65. {
  66.     motionreq_n.acct = ta;
  67.     motionreq_n.sgt = ts;
  68. }
  69.  
  70.  
  71. setvel(t, r) /*::*/
  72. int t, r;
  73. {
  74.     motionreq_n.velt = t;
  75.     motionreq_n.velr = r;
  76. }
  77.  
  78.  
  79. evalfn(fn) /*::*/
  80. TRFN fn;
  81. {
  82.     motionreq_n.fnt = fn;
  83. }
  84.  
  85.  
  86. setconf(s) /*::*/
  87. char *s;
  88. {
  89.     motionreq_n.cfg = s;
  90. }
  91.  
  92.  
  93.  
  94. sample(m) /*::*/
  95. int m;
  96. {
  97.     if (m < MINSAMPLE) {
  98.         m = MINSAMPLE;
  99.     }
  100.     if (m > MAXSAMPLE) {
  101.         m = MAXSAMPLE;
  102.     }
  103.     motionreq_n.smpl = m - m % HARDCLOCK;
  104. }
  105.  
  106.  
  107. massis(m) /*::*/
  108. real m;
  109. {
  110.     motionreq_n.fmass = m * 9.81;
  111. }
  112.  
  113.  
  114. /*
  115.  * this one is a special case, only a TERM_PTR need to be transmitted
  116.  * to the setpoint for solving, the corresponding term is set in movereq
  117.  */
  118.  
  119. update(u, p) /*::*/
  120. TRSF_PTR u;
  121. POS_PTR p;
  122. {
  123.     updtr = u;
  124.     updpos = (PST_PTR)p;
  125. }
  126.  
  127.  
  128. #define NmTONmm         1000.
  129. #define DegTORad        0.01745329251994330
  130.  
  131. /*
  132.  * parses the argument string, and set the motion record limit entries
  133.  */
  134.  
  135. /* VARARGS */
  136. limit(c, a) /*::*/
  137. char *c;
  138. double a;
  139. {
  140.     double *pa = &a;
  141.  
  142.     while (*c) {
  143.         switch (*c++) {
  144.         case 'f' :
  145.             switch (*c++) {
  146.             case 'x' :
  147.                 motionreq_n.fsp |= SELFX;
  148.                 motionreq_n.frc.f.x = *pa++;
  149.                 break;
  150.  
  151.             case 'y' :
  152.                 motionreq_n.fsp |= SELFY;
  153.                 motionreq_n.frc.f.y = *pa++;
  154.                 break;
  155.  
  156.             case 'z' :
  157.                 motionreq_n.fsp |= SELFZ;
  158.                 motionreq_n.frc.f.z = *pa++;
  159.                 break;
  160.  
  161.             default :
  162.                 goto bad;
  163.             }
  164.             break;
  165.  
  166.         case 't' :
  167.             switch (*c++) {
  168.             case 'x' :
  169.                 motionreq_n.fsp |= SELMX;
  170.                 motionreq_n.frc.m.x = *pa++ * NmTONmm;
  171.                 break;
  172.  
  173.             case 'y' :
  174.                 motionreq_n.fsp |= SELMY;
  175.                 motionreq_n.frc.m.y = *pa++ * NmTONmm;
  176.                 break;
  177.  
  178.             case 'z' :
  179.                 motionreq_n.fsp |= SELMZ;
  180.                 motionreq_n.frc.m.z = *pa++ * NmTONmm;
  181.                 break;
  182.  
  183.             default :
  184.                 goto bad;
  185.             }
  186.             break;
  187.  
  188.         case 'd' :
  189.             switch (*c++) {
  190.             case 'x' :
  191.                 motionreq_n.dsp |= SELTX;
  192.                 motionreq_n.dst.t.x = *pa++;
  193.                 break;
  194.  
  195.             case 'y' :
  196.                 motionreq_n.dsp |= SELTY;
  197.                 motionreq_n.dst.t.y = *pa++;
  198.                 break;
  199.  
  200.             case 'z' :
  201.                 motionreq_n.dsp |= SELTZ;
  202.                 motionreq_n.dst.t.z = *pa++;
  203.                 break;
  204.  
  205.             default :
  206.                 goto bad;
  207.             }
  208.             break;
  209.  
  210.         case 'r' :
  211.             switch (*c++) {
  212.             case 'x' :
  213.                 motionreq_n.dsp |= SELRX;
  214.                 motionreq_n.dst.r.x = *pa++ * DegTORad;
  215.                 break;
  216.  
  217.             case 'y' :
  218.                 motionreq_n.dsp |= SELRY;
  219.                 motionreq_n.dst.r.y = *pa++ * DegTORad;
  220.                 break;
  221.  
  222.             case 'z' :
  223.                 motionreq_n.dsp |= SELRZ;
  224.                 motionreq_n.dst.r.z = *pa++ * DegTORad;
  225.                 break;
  226.  
  227.             default :
  228.                 goto bad;
  229.             }
  230.             break;
  231.  
  232.         case ' ' :
  233.             break;
  234.  
  235.         default :
  236.             goto bad;
  237.         }
  238.     }
  239.     return;
  240. bad :
  241.     giveup("bad spec. - limit", YES);
  242. }
  243.  
  244.  
  245.  
  246. /*
  247.  * comply and lock parse the string argument and the set the comply entry
  248.  * perform set operations to make sure specs are consistent
  249.  */
  250.  
  251. /* VARARGS */
  252. comply(c, a) /*::*/
  253. char *c;
  254. double a;
  255. {
  256.     double *pa = &a;
  257.  
  258.     while (*c) {
  259.         switch (*c++) {
  260.         case 'f' :
  261.             switch (*c++) {
  262.             case 'x' :
  263.                 if (motionreq_n.csp & SELFX) {
  264.                     goto bad;
  265.                 }
  266.                 motionreq_n.csp |= SELFX;
  267.                 motionreq_n.cpy.f.x = *pa++;
  268.                 break;
  269.  
  270.             case 'y' :
  271.                 if (motionreq_n.csp & SELFY) {
  272.                     goto bad;
  273.                 }
  274.                 motionreq_n.csp |= SELFY;
  275.                 motionreq_n.cpy.f.y = *pa++;
  276.                 break;
  277.  
  278.             case 'z' :
  279.                 if (motionreq_n.csp & SELFZ) {
  280.                     goto bad;
  281.                 }
  282.                 motionreq_n.csp |= SELFZ;
  283.                 motionreq_n.cpy.f.z = *pa++;
  284.                 break;
  285.  
  286.             default :
  287.                 goto bad;
  288.             }
  289.             break;
  290.  
  291.         case 't' :
  292.             switch (*c++) {
  293.             case 'x' :
  294.                 if (motionreq_n.csp & SELMX) {
  295.                     goto bad;
  296.                 }
  297.                 motionreq_n.csp |= SELMX;
  298.                 motionreq_n.cpy.m.x = *pa++ * NmTONmm;
  299.                 break;
  300.  
  301.             case 'y' :
  302.                 if (motionreq_n.csp & SELMY) {
  303.                     goto bad;
  304.                 }
  305.                 motionreq_n.csp |= SELMY;
  306.                 motionreq_n.cpy.m.y = *pa++ * NmTONmm;
  307.                 break;
  308.  
  309.             case 'z' :
  310.                 if (motionreq_n.csp & SELMZ) {
  311.                     goto bad;
  312.                 }
  313.                 motionreq_n.csp |= SELMZ;
  314.                 motionreq_n.cpy.m.z = *pa++ * NmTONmm;
  315.                 break;
  316.  
  317.             default :
  318.                 goto bad;
  319.             }
  320.             break;
  321.  
  322.         case ' ' :
  323.             break;
  324.  
  325.         default :
  326.             goto bad;
  327.         }
  328.     }
  329.     return;
  330. bad :
  331.     giveup("bad force spec. - comply", YES);
  332. }
  333.  
  334.  
  335.  
  336.  
  337. lock(c) /*::*/
  338. char *c;
  339. {
  340.     while (*c) {
  341.         switch (*c++) {
  342.         case 'f' :
  343.             switch (*c++) {
  344.             case 'x' :
  345.                 if (!(motionreq_n.csp & SELFX)) {
  346.                     goto bad;
  347.                 }
  348.                 motionreq_n.csp &= ~SELFX;
  349.                 motionreq_n.cpy.f.x = 0.;
  350.                 break;
  351.  
  352.             case 'y' :
  353.                 if (!(motionreq_n.csp & SELFY)) {
  354.                     goto bad;
  355.                 }
  356.                 motionreq_n.csp &= ~SELFY;
  357.                 motionreq_n.cpy.f.y = 0.;
  358.                 break;
  359.  
  360.             case 'z' :
  361.                 if (!(motionreq_n.csp & SELFZ)) {
  362.                     goto bad;
  363.                 }
  364.                 motionreq_n.csp &= ~SELFZ;
  365.                 motionreq_n.cpy.f.z = 0.;
  366.                 break;
  367.  
  368.             default :
  369.                 goto bad;
  370.             }
  371.             break;
  372.  
  373.         case 't' :
  374.             switch (*c++) {
  375.             case 'x' :
  376.                 if (!(motionreq_n.csp & SELMX)) {
  377.                     goto bad;
  378.                 }
  379.                 motionreq_n.csp &= ~SELMX;
  380.                 motionreq_n.cpy.m.x = 0.;
  381.                 break;
  382.  
  383.             case 'y' :
  384.                 if (!(motionreq_n.csp & SELMY)) {
  385.                     goto bad;
  386.                 }
  387.                 motionreq_n.csp &= ~SELMY;
  388.                 motionreq_n.cpy.m.y = 0.;
  389.                 break;
  390.  
  391.             case 'z' :
  392.                 if (!(motionreq_n.csp & SELMZ)) {
  393.                     goto bad;
  394.                 }
  395.                 motionreq_n.csp &= ~SELMZ;
  396.                 motionreq_n.cpy.m.z = 0.;
  397.                 break;
  398.  
  399.             default :
  400.                 goto bad;
  401.             }
  402.             break;
  403.  
  404.         case ' ' :
  405.             break;
  406.  
  407.         default :
  408.             goto bad;
  409.         }
  410.     }
  411.     return;
  412. bad :
  413.     giveup("bad force spec. - lock", YES);
  414. }
  415.  
  416.  
  417.  
  418. /*
  419.  * same principle as above but for distances, no check necessary
  420.  */
  421.  
  422. /* VARARGS */
  423. distance(c, a) /*::*/
  424. char *c;
  425. double a;
  426. {
  427.     double *pa = &a;
  428.  
  429.     while (*c) {
  430.         switch (*c++) {
  431.         case 'd' :
  432.             switch (*c++) {
  433.             case 'x' :
  434.                 motionreq_n.exp |= SELTX;
  435.                 motionreq_n.exd.t.x = *pa++;
  436.                 break;
  437.  
  438.             case 'y' :
  439.                 motionreq_n.exp |= SELTY;
  440.                 motionreq_n.exd.t.y = *pa++;
  441.                 break;
  442.  
  443.             case 'z' :
  444.                 motionreq_n.exp |= SELTZ;
  445.                 motionreq_n.exd.t.z = *pa++;
  446.                 break;
  447.  
  448.             default :
  449.                 goto bad;
  450.             }
  451.             break;
  452.  
  453.         case 'r' :
  454.             switch (*c++) {
  455.             case 'x' :
  456.                 motionreq_n.exp |= SELRX;
  457.                 motionreq_n.exd.r.x = *pa++ * DegTORad;
  458.                 break;
  459.  
  460.             case 'y' :
  461.                 motionreq_n.exp |= SELRY;
  462.                 motionreq_n.exd.r.y = *pa++ * DegTORad;
  463.                 break;
  464.  
  465.             case 'z' :
  466.                 motionreq_n.exp |= SELRZ;
  467.                 motionreq_n.exd.r.z = *pa++ * DegTORad;
  468.                 break;
  469.  
  470.             default :
  471.                 goto bad;
  472.             }
  473.             break;
  474.  
  475.         case ' ' :
  476.             break;
  477.  
  478.         default :
  479.             goto bad;
  480.         }
  481.     }
  482.     return;
  483. bad :
  484.     giveup("bad distance spec. - distance", YES);
  485. }
  486.  
  487.  
  488.  
  489.  
  490. /*
  491.  * move, set up the  position and issue the request
  492.  */
  493.  
  494. move(p) /*::*/
  495. POS_PTR p;
  496. {
  497.     motionreq_n.pst = (PST_PTR)p;
  498.     movereq();
  499. }
  500.  
  501.  
  502. /*
  503.  * stop issue the request without pos, means redo the last one
  504.  */
  505.  
  506. stop(t) /*::*/
  507. int t;
  508. {
  509.     motionreq_n.sgt = t;
  510.     movereq();
  511. }
  512.  
  513.  
  514. /*
  515.  * 1) make sure specs are consitent
  516.  * 2) print the requests
  517.  * 3) put them in the queue
  518.  * 4) reset the record
  519.  */
  520.  
  521. movereq() /*::*/
  522. {
  523.     static int sam = DEF_SAMPLE;
  524.     static FORCE zeroforce = {0., 0., 0., 0., 0., 0.};
  525.     static DIFF zerodiff = {0., 0., 0., 0., 0., 0.};
  526.  
  527.     char *new();
  528.  
  529.     if (*(motionreq_n.cfg) != '\0' && motionreq_n.mod != 'j') {
  530.         giveup("conf must change in joint mode", YES);
  531.     }
  532.     if (motionreq_n.mod != 'j' && motionreq_n.mod != 'c') {
  533.         motionreq_n.mod = 'j';
  534.     }
  535.  
  536.     if (motionreq_n.smpl != 0) {
  537.         sam = motionreq_n.smpl;
  538.     }
  539.     motionreq_n.acct /= sam;
  540.     motionreq_n.sgt /= sam;
  541.  
  542.     if (motionreq_n.sgt == 0) {
  543.         if (motionreq_n.velt <= 0) {
  544.             motionreq_n.velt = DEF_TRSV;
  545.         }
  546.         if (motionreq_n.velr <= 0) {
  547.             motionreq_n.velr = DEF_ROTV;
  548.         }
  549.         if (motionreq_n.acct == 0) {
  550.             motionreq_n.acct = 2 +
  551.             ((motionreq_n.velt + motionreq_n.velr) / (20 * sam));
  552.         }
  553.     }
  554.     else {
  555.         if (motionreq_n.acct == 1) {
  556.             motionreq_n.acct = 2;
  557.         }
  558.         if (2 * motionreq_n.acct > motionreq_n.sgt) {
  559.             motionreq_n.sgt = 2 * motionreq_n.acct;
  560.         }
  561.     }
  562.     if (motionreq_n.pst != NULL) {
  563.         PST_PTR p = motionreq_n.pst;
  564.         TERM_PTR t;
  565.         TRSF_PTR h;
  566.         ITEM_PTR ih;
  567.  
  568.         for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
  569.             if (t->trsf->fn == hold) {
  570.                 if ((ih = (ITEM_PTR)new(sizeof(ITEM) +
  571.                                sizeof(TRSF)))
  572.                        == NULL) {
  573.                     giveup("alloc err", YES);
  574.                 }
  575.                 h = (TRSF_PTR)((char *)ih + sizeof(ITEM));
  576.                 Assigntr(h, t->trsf);
  577.                 enqueue_n(&(t->hd), ih);
  578.             }
  579.         }
  580.         if (updtr != NULL) {
  581.             if (updtr->fn != varb) {
  582.                 giveup("invalid update transform type", YES);
  583.             }
  584.             p = updpos;
  585.             for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
  586.                 if (t->trsf == updtr) {
  587.                     motionreq_n.upd = t;
  588.                     break;
  589.                 }
  590.             }
  591.             if (motionreq_n.upd == NULL) {
  592.                 giveup("could'nt find updatable transform", YES);
  593.             }
  594.         }
  595.     }
  596.  
  597.     if (prints_out) {
  598.         fprintf(fpi,
  599.         "request %s mode %c acct %d  sgt %d  velt %d velr %d\n",
  600.             (motionreq_n.pst) ? motionreq_n.pst->name : "STOP",
  601.             motionreq_n.mod,
  602.             motionreq_n.acct * sam,
  603.             motionreq_n.sgt * sam,
  604.             motionreq_n.velt,
  605.             motionreq_n.velr);
  606.         fprintf(fpi,
  607.         "conf %s  %s upd : %s smpl %d mass %f\n",
  608.             motionreq_n.cfg,
  609.             (motionreq_n.fnt) ? "EVAL" : "",
  610.             (motionreq_n.upd) ? motionreq_n.upd->trsf->name : "",
  611.             motionreq_n.smpl,
  612.             motionreq_n.fmass);
  613.         if (motionreq_n.fsp & SELFX) {
  614.             fprintf(fpi, "stop fx : %-5g\n", motionreq_n.frc.f.x);
  615.         }
  616.         if (motionreq_n.fsp & SELFY) {
  617.             fprintf(fpi, "stop fy : %-5g\n", motionreq_n.frc.f.y);
  618.         }
  619.         if (motionreq_n.fsp & SELFZ) {
  620.             fprintf(fpi, "stop fz : %-5g\n", motionreq_n.frc.f.z);
  621.         }
  622.         if (motionreq_n.fsp & SELMX) {
  623.             fprintf(fpi, "stop mx : %-5g\n", motionreq_n.frc.m.x);
  624.         }
  625.         if (motionreq_n.fsp & SELMY) {
  626.             fprintf(fpi, "stop my : %-5g\n", motionreq_n.frc.m.y);
  627.         }
  628.         if (motionreq_n.fsp & SELMZ) {
  629.             fprintf(fpi, "stop mz : %-5g\n", motionreq_n.frc.m.z);
  630.         }
  631.  
  632.         if (motionreq_n.csp & SELFX) {
  633.             fprintf(fpi, "cply fx : %-5g\n", motionreq_n.cpy.f.x);
  634.         }
  635.         if (motionreq_n.csp & SELFY) {
  636.             fprintf(fpi, "cply fy : %-5g\n", motionreq_n.cpy.f.y);
  637.         }
  638.         if (motionreq_n.csp & SELFZ) {
  639.             fprintf(fpi, "cply fz : %-5g\n", motionreq_n.cpy.f.z);
  640.         }
  641.         if (motionreq_n.csp & SELMX) {
  642.             fprintf(fpi, "cply mx : %-5g\n", motionreq_n.cpy.m.x);
  643.         }
  644.         if (motionreq_n.csp & SELMY) {
  645.             fprintf(fpi, "cply my : %-5g\n", motionreq_n.cpy.m.y);
  646.         }
  647.         if (motionreq_n.csp & SELMZ) {
  648.             fprintf(fpi, "cply mz : %-5g\n", motionreq_n.cpy.m.z);
  649.         }
  650.  
  651.         if (motionreq_n.dsp & SELTX) {
  652.             fprintf(fpi, "stop dx : %-5g\n", motionreq_n.dst.t.x);
  653.         }
  654.         if (motionreq_n.dsp & SELTY) {
  655.             fprintf(fpi, "stop dy : %-5g\n", motionreq_n.dst.t.y);
  656.         }
  657.         if (motionreq_n.dsp & SELTZ) {
  658.             fprintf(fpi, "stop dz : %-5g\n", motionreq_n.dst.t.z);
  659.         }
  660.         if (motionreq_n.dsp & SELRX) {
  661.             fprintf(fpi, "stop rx : %-5g\n", motionreq_n.dst.r.x);
  662.         }
  663.         if (motionreq_n.dsp & SELRY) {
  664.             fprintf(fpi, "stop ry : %-5g\n", motionreq_n.dst.r.y);
  665.         }
  666.         if (motionreq_n.dsp & SELRZ) {
  667.             fprintf(fpi, "stop rz : %-5g\n", motionreq_n.dst.r.z);
  668.         }
  669.  
  670.         if (motionreq_n.exp & SELTX) {
  671.             fprintf(fpi, "dist dx : %-5g\n", motionreq_n.exd.t.x);
  672.         }
  673.         if (motionreq_n.exp & SELTY) {
  674.             fprintf(fpi, "dist dy : %-5g\n", motionreq_n.exd.t.y);
  675.         }
  676.         if (motionreq_n.exp & SELTZ) {
  677.             fprintf(fpi, "dist dz : %-5g\n", motionreq_n.exd.t.z);
  678.         }
  679.         if (motionreq_n.exp & SELRX) {
  680.             fprintf(fpi, "dist rx : %-5g\n", motionreq_n.exd.r.x);
  681.         }
  682.         if (motionreq_n.exp & SELRY) {
  683.             fprintf(fpi, "dist ry : %-5g\n", motionreq_n.exd.r.y);
  684.         }
  685.         if (motionreq_n.exp & SELRZ) {
  686.             fprintf(fpi, "dist rz : %-5g\n", motionreq_n.exd.r.z);
  687.         }
  688.         fprintf(fpi, "\n");
  689.     }
  690.  
  691.     ++requestnb;
  692.  
  693.     {
  694.         ITEM_PTR im;
  695.         MOT_PTR m;
  696.  
  697.         if ((im = (ITEM_PTR)new(sizeof(ITEM) + sizeof(MOT))) == NULL) {
  698.             giveup("alloc err", YES);
  699.         }
  700.         m = (MOT_PTR)((char *)im + sizeof(ITEM));
  701.  
  702.         m->acct = motionreq_n.acct;
  703.         m->sgt = motionreq_n.sgt;
  704.         m->velt = motionreq_n.velt;
  705.         m->velr = motionreq_n.velr;
  706.         m->pst = motionreq_n.pst;
  707.         m->mod = motionreq_n.mod;
  708.         m->cfg = motionreq_n.cfg;
  709.         m->fnt = motionreq_n.fnt;
  710.         m->upd = motionreq_n.upd;
  711.         m->smpl = motionreq_n.smpl;
  712.         m->fmass = motionreq_n.fmass;
  713.         m->fsp = motionreq_n.fsp;
  714.         Assignforce(&m->frc, &motionreq_n.frc);
  715.         m->csp = motionreq_n.csp;
  716.         Assignforce(&m->cpy, &motionreq_n.cpy);
  717.         m->dsp = motionreq_n.dsp;
  718.         Assigndiff(&m->dst, &motionreq_n.dst);
  719.         m->exp = motionreq_n.exp;
  720.         Assigndiff(&m->exd, &motionreq_n.exd);
  721.  
  722.         enqueue_n(&mqueue_n, im);
  723.     }
  724.     motionreq_n.acct = 0;
  725.     motionreq_n.sgt = 0;
  726.     /* no velocities reset */
  727.     motionreq_n.pst = NULL;
  728.     motionreq_n.cfg = "";
  729.     motionreq_n.upd = NULL;
  730.     motionreq_n.fnt = NULL;
  731.     updtr = NULL;
  732.     updpos = NULL;
  733.     motionreq_n.smpl = 0;
  734.     /* no mass reset */
  735.     motionreq_n.fsp = 0;
  736.     Assignforce(&motionreq_n.frc, &zeroforce);
  737.     /* no comply stuff reset */
  738.     motionreq_n.dsp = 0;
  739.     Assigndiff(&motionreq_n.dst, &zerodiff);
  740.     motionreq_n.exp = 0;
  741.     Assigndiff(&motionreq_n.exd, &zerodiff);
  742. }
  743.  
  744.  
  745.  
  746.  
  747.  
  748.  
  749. /*
  750.  * queue and dequeue motion requests
  751.  * queues have a header with two pointers put and get and
  752.  * are a singly linked list with one pointer and a data field
  753.  * items of any size can be handled
  754.  * enqueue uses a pointer to a preallocated area where the data is stored
  755.  * dequeue returns a pointer to the data and free the allocated area
  756.  */
  757.  
  758. static int enqbusy = NO, deqbusy = NO;
  759.  
  760.  
  761. enqueue_n(r, l) /*:: put a record in a queue */
  762. register ROOT_PTR r;
  763. register ITEM_PTR l;
  764. {
  765.     enqbusy = YES;
  766.     while (deqbusy) {
  767.         fprintf(stderr, "*** could'nt queue at %d\n", rtime);
  768.         nap(5);
  769.            /*NOTREACHED*/
  770.     }
  771.     if (r->put == NULL) {
  772.         r->get = l;
  773.     }
  774.     else {
  775.         r->put->next = l;
  776.     }
  777.     r->put = l;
  778.     r->put->next = NULL;
  779.     enqbusy = NO;
  780. }
  781.  
  782.  
  783.  
  784. ITEM_PTR dequeue_n(r) /*::*/
  785. register ROOT_PTR r;
  786. {
  787.     register ITEM_PTR g;
  788.  
  789.     deqbusy = YES;
  790.     if (enqbusy) {
  791.         deqbusy = NO;
  792.         return(NULL);   /* stop the arm */
  793.     }
  794.     if (r->get == NULL) {
  795.         deqbusy = NO;
  796.         return(NULL);
  797.     }
  798.     g = r->get;
  799.     r->get = r->get->next;
  800.     if (r->get == NULL) {
  801.         r->put = NULL;
  802.     }
  803.     disp((char *)g);
  804.     deqbusy = NO;
  805.     return((ITEM_PTR)((char *)g + sizeof(ITEM)));
  806. }
  807.  
  808.  
  809.  
  810. /*
  811.  * storage allocator for the private use of queue/dequeue
  812.  * inspired from Kernighan
  813.  */
  814.  
  815. #define NPOOL   4096
  816.  
  817. struct s {
  818.     union head *ptr;
  819.     unsigned size;
  820. };
  821.  
  822. typedef union head {
  823.     struct s s;
  824.     ALIGN   x;
  825. } HEAD;
  826.  
  827. typedef struct pl {
  828.     struct s s;
  829.     HEAD st[NPOOL];
  830. } POOL;
  831.  
  832. static POOL pool;
  833. static HEAD *allocp = NULL;
  834.  
  835.  
  836. static char *new(nb) /*##*/
  837. int nb;
  838. {
  839.     register HEAD *p, *q;
  840.     register int nunits = 1 + (nb + sizeof(HEAD) - 1) / sizeof(HEAD);
  841.  
  842.     if ((q = allocp) == NULL) {
  843.         pool.s.ptr = allocp = q = (HEAD *)&pool;
  844.         pool.s.size = NPOOL;
  845.     }
  846.     for (p = q->s.ptr; ; q = p, p = p->s.ptr) {
  847.         if (p->s.size >= nunits) {
  848.             if (p->s.size == nunits) {
  849.                 q->s.ptr = p->s.ptr;
  850.             }
  851.             else {
  852.                 p->s.size -= nunits;
  853.                 p += p->s.size;
  854.                 p->s.size = nunits;
  855.             }
  856.             allocp = q;
  857.             return((char *)(p + 1));
  858.         }
  859.         if (p == allocp) {
  860.             return(NULL);
  861.         }
  862.     }
  863. }
  864.  
  865.  
  866.  
  867. static disp(ap)  /*##*/
  868. register char *ap;
  869. {
  870.     register HEAD *p, *q;
  871.  
  872.     p = (HEAD *) ap - 1;
  873.     for (q = allocp; !(p > q && p < q->s.ptr); q = q->s.ptr)
  874.         if (q >= q->s.ptr && (p > q || p < q->s.ptr))
  875.             break;
  876.     if (p + p->s.size == q->s.ptr) {
  877.         p->s.size += q->s.ptr->s.size;
  878.         p->s.ptr = q->s.ptr->s.ptr;
  879.     }
  880.     else {
  881.         p->s.ptr = q->s.ptr;
  882.     }
  883.     if (q + q->s.size == p) {
  884.         q->s.size += p->s.size;
  885.         q->s.ptr = p->s.ptr;
  886.     }
  887.     else {
  888.         q->s.ptr = p;
  889.     }
  890.     allocp = q;
  891. }
  892.