home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl096 < prev    next >
Text File  |  1987-03-02  |  18KB  |  860 lines
  1. /*
  2.  * RTC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : rtc
  6.  *      File    : pack.c
  7.  *      Remarks : Contains all the robot interface: channel setup,
  8.  *                data checking, command translation, arm monitoring, and
  9.  *                user interface.
  10.  *                Also the manual step mode and home return.
  11.  *      Usage   : part of the library
  12.  */
  13.  
  14. /*LINTLIBRARY*/
  15.  
  16. #include <stdio.h>
  17. #include <sys/types.h>
  18. #include <sys/drc.h>
  19. #include <signal.h>
  20. #include <sgtty.h>
  21. #include "../h/exiod.h"
  22. #include "../h/cmdk.h"
  23. #include "../h/fifoio.h"
  24. #include "../h/which.h"
  25. #include "../h/rtc.h"
  26. #include "../h/umac.h"
  27. #ifdef PUMA
  28. #include "../h/pumadata.h"
  29. #endif
  30. #ifdef STAN
  31. #include "../h/standata.h"
  32. #endif
  33.  
  34.  
  35. struct chg chg;         /* global control  structure    */
  36.  
  37. static struct fifobuf cmdbuf = {2, VERSION, ARMTYPE};
  38.  
  39. union howu {
  40.     struct fifobuf howb;
  41.     struct how     howw;
  42. } how;
  43.  
  44. int terminate = NO;     /* shuts up the pack routine    */
  45. char *mess;             /* printed by release           */
  46. int Magic_Circus = YES; /* secret entry point           */
  47.  
  48.  
  49. static unsigned short ref[NJOINTS] = {ECCL1, ECCL2, ECCL3, ECCL4, ECCL5, ECCL6};
  50. static unsigned short idx[NJOINTS] = {XCCL1, XCCL2, XCCL3, XCCL4, XCCL5, XCCL6};
  51. static unsigned short max[NJOINTS] = {ECMX1, ECMX2, ECMX3, ECMX4, ECMX5, ECMX6};
  52. static unsigned short min[NJOINTS] = {ECMN1, ECMN2, ECMN3, ECMN4, ECMN5, ECMN6};
  53. static unsigned short mvl[NJOINTS] = {EMXV1, EMXV2, EMXV3, EMXV4, EMXV5, EMXV6};
  54. static unsigned short mxdc[NJOINTS] = {MXDC1, MXDC2, MXDC3, MXDC4, MXDC5, MXDC6}
  55. static unsigned short mxoc[NJOINTS] = {MXOC1, MXOC2, MXOC3, MXOC4, MXOC5, MXOC6}
  56. #ifdef ADC
  57. static int nch = NJOINTS;
  58. static int adcmap[ADC] =
  59.         {0, 1, 2, 3, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
  60. #endif
  61. static int rate;
  62. static int inserf = NO;
  63. static int active = NO;
  64. static int first = YES;
  65. static int cbrkset = NO;
  66. static int checking = YES;
  67. static struct sgttyb mode;
  68.  
  69. /*
  70.  * set of macros to test requests contained in the chg structure
  71.  * and fill the output buffer
  72.  */
  73.  
  74. #define IRQST(x)        *(k = &chg. x [i].set)
  75.  
  76. #define STUFFI(x,C)     if (stack > top - 4)\
  77.                 goto fatal;\
  78.             *stack++ = C | i;\
  79.             *stack++ = chg. x [i].vali;\
  80.             *k = NO;
  81.  
  82. #define CMDI(x,C)       if(IRQST(x)) {STUFFI(x,C)}
  83.  
  84. #define ARQST(x)        *(k = &chg. x .set)
  85.  
  86. #define STUFFA(x,C)     if (stack > top - NJOINTS + 2)\
  87.                 goto fatal;\
  88.             *stack++ = C;\
  89.             for (i = 0; i < NJOINTS; ++i) {\
  90.                 *stack++ = chg. x .vala[i];\
  91.             } *k = NO;
  92.  
  93. #define CMDA(x,C)       if(ARQST(x)) {STUFFA(x,C)}
  94.  
  95. #define GRQST(x)        *(k = &chg. x .set)
  96.  
  97. #define STUFFG(x,C)     if (stack > top - 4)\
  98.                 goto fatal;\
  99.             *stack++ = C;\
  100.             *stack++ = chg. x .valg;\
  101.             *k = NO;
  102.  
  103. #define CMDG(x,C)       if(GRQST(x)) {STUFFG(x,C)}
  104.  
  105. #define RQSTE(x)        *(k = &chg. x)
  106.  
  107. #define STUFFE(C)       if (stack > top - 2)\
  108.                 goto fatal;\
  109.             *stack++ = C;\
  110.             *k = NO;
  111.  
  112. #define CMDE(x,C)       if (RQSTE(x)) {STUFFE(C)}
  113.  
  114. /*
  115.  * the pack function is called after execution of the interrupt
  116.  * user function 1 to translates the requests expressed in the
  117.  * chg structure into the communication language understandable
  118.  * by the lsi code.
  119.  * If for a reason or another, the flag terminate is raised,
  120.  * the user interrupt functions will no longer be called and
  121.  * pack will refuse to send anything but a rate command witc neg val which
  122.  * cause the lsi code to stop interrupting the vax and turn off
  123.  * the arm power.
  124.  * If the power is turned off, interrupts are ignored and execution
  125.  * resume when power is back on.
  126.  * To add more commands, simply add more chg strcture entries, corresponding
  127.  * macro calls in pack, resets in control and interpretations in moper.
  128.  */
  129.  
  130.  
  131. static pack() /*##*/
  132. {
  133.     register int i;
  134.     register char *k;
  135.     register short *stack = cmdbuf.data;
  136.     register short *top = cmdbuf.data + FIFOBUFS;
  137.     register int inc;
  138.     register unsigned short mcc;
  139.  
  140.     if (terminate) {
  141. terminated:     chg.end = YES;
  142.         CMDE(end, END_E)
  143.         return(stack - cmdbuf.data);    /* return word count */
  144.     }
  145.  
  146.     for (i = 0; i < NJOINTS; ++i) {
  147.         switch (IRQST(i_motion)) {
  148.         case NO :
  149.             break;
  150.  
  151.         case POS :
  152.             inc = chg.i_motion[i].vali - how.howw.pos[i];
  153.             inc = (inc < 0) ? -inc : inc;
  154.             if (checking) {
  155.                 if (inc > mvl[i] * rate) {
  156.                     goto toofast;
  157.                 }
  158.                 inc = chg.i_motion[i].vali;
  159.                 if (inc > max[i] || inc < min[i]) {
  160.                     goto toofar;
  161.                 }
  162.             }
  163.             STUFFI(i_motion,POS_I)
  164.             break;
  165.  
  166.         case CUR :
  167.             mcc = chg.i_motion[i].vali;
  168.             mcc = (mcc > 0100000) ? -mcc : mcc;
  169.             if (mcc > mxdc[i]) {
  170.                 goto toostrong;
  171.             }
  172.             STUFFI(i_motion,CUR_I)
  173.             break;
  174.  
  175.         case STOP :
  176.             STUFFI(i_motion,STOPP_I);
  177.             break;
  178.  
  179.         case STOPCAL :
  180.             STUFFI(i_motion,STOPM_I);
  181.             break;
  182.         default :
  183.             goto fatal;
  184.         }
  185.         CMDI(i_gravty,GRAV_I);
  186.     }
  187.  
  188.     switch (ARQST(a_motion)) {
  189.     case NO :
  190.         break;
  191.  
  192.     case POS :
  193.         if (checking) {
  194.             for (i = 0; i < NJOINTS; ++i) {
  195.                 inc = chg.a_motion.vala[i] - how.howw.pos[i];
  196.                 inc = (inc < 0) ? -inc : inc;
  197.                 if (inc > mvl[i] * rate) {
  198.                     goto toofast;
  199.                 }
  200.                 inc = chg.a_motion.vala[i];
  201.                 if (inc > max[i] || inc < min[i]) {
  202.                     goto toofar;
  203.                 }
  204.             }
  205.         }
  206.         STUFFA(a_motion,POS_A)
  207.         break;
  208.  
  209.     case CUR :
  210.         for (i = 0; i < NJOINTS; ++i) {
  211.             mcc = chg.a_motion.vala[i];
  212.             mcc = (mcc > 0100000) ? -mcc : mcc;
  213.             if (mcc > mxdc[i]) {
  214.                 goto toostrong;
  215.             }
  216.         }
  217.         STUFFA(a_motion,CUR_A);
  218.         break;
  219.     case STOP :
  220.         STUFFA(a_motion,STOPP_A);
  221.         break;
  222.     case STOPCAL :
  223.         STUFFA(a_motion,STOPM_A);
  224.         break;
  225.     default :
  226.         goto fatal;
  227.     }
  228.  
  229.     CMDA(a_gravty, GRAV_A);
  230.  
  231.     CMDG(g_hand, HAND_G);
  232. #ifdef ADC
  233.     if (chg.g_adco.set) {
  234.         ++nch;
  235.     }
  236.     CMDG(g_adco, ADCO_G)
  237. #endif
  238.     CMDG(g_rate, RATE_G)
  239.     if (chg.g_rate.set) {
  240.         rate = (1 << chg.g_rate.valg) * HARDCLOCK;
  241.     }
  242.     CMDE(stop, STOP_E)
  243.     CMDE(calib, CALIB_E)
  244.  
  245.     return(stack - cmdbuf.data);    /* return word count */
  246.  
  247. fatal :
  248.     stack = cmdbuf.data;
  249.     mess = "** too many commands";
  250.     terminate = YES;
  251.     goto terminated;
  252.  
  253. toofast :
  254.     stack = cmdbuf.data;
  255.     terminate = YES;
  256.     mess = "** too large position increment";
  257.     goto terminated;
  258.  
  259. toofar :
  260.     stack = cmdbuf.data;
  261.     terminate = YES;
  262.     mess = "** out of range";
  263.     goto terminated;
  264.  
  265. toostrong :
  266.     stack = cmdbuf.data;
  267.     terminate = YES;
  268.     mess = "** too much current";
  269.     goto terminated;
  270. }
  271.  
  272.  
  273. /*
  274.  * - set up the driver
  275.  * - have the SIGINT caught to terminate interaction
  276.  * - have the SIGHUP caught on device error
  277.  *
  278.  * On IT :
  279.  * 1) The driver gets a buffer from the controller
  280.  * 2) User fn1 is called and is meant to process the arm state
  281.  * 3) Commands are put in buffer by pack
  282.  * 3) The driver sends a buffer to the controller (previous commands)
  283.  * 4) User fn2 is called and is meant to compute next command
  284.  *
  285.  * One may code any sequence of 'control-release' in the user programs
  286.  * Each of these sequence corresponds to a sequence
  287.  * 'interrupt/send state/execute commands - turn off armpower in the lsi'
  288.  */
  289.  
  290. static dummy(){} /*##*/
  291.  
  292. #define BAD     2
  293. #define TOOF    3
  294.  
  295. static struct drcROBOT drcROBOT  = {
  296.     (caddr_t) &cmdbuf,      /* this write buffer */
  297.     (caddr_t) &how,         /* this is read buffer */
  298.     dummy,
  299.     dummy
  300. };
  301.  
  302.  
  303. static int fdr;
  304. static int (* userfn1)();
  305. static int (* userfn2)();
  306. static int exch  = 0;
  307. static int ntest = NTEST;
  308. static int hang;
  309.  
  310. control(fn1, fn2) /*::*/
  311. int (* fn1)(), (* fn2)();
  312. {
  313.     int count;
  314.     int release();
  315.     int ondrcerr();
  316.     int onintr1();
  317.     int onintr2();
  318.     char *ttyname();
  319.  
  320.     if (active) {
  321.         terminate = YES;
  322.         mess = "** exit";
  323.         release("** channel not released");
  324.         exit(4);
  325.     }
  326. #ifdef PUMA
  327.     if (strcmp(ttyname(0), "/dev/tty02") != 0) {
  328.         printf("not the Puma's terminal\n");
  329.         exit(5);
  330.     }
  331. #endif
  332. #ifdef STAN
  333.     if (strcmp(ttyname(0), "/dev/tty03") != 0) {
  334.         printf("not the Stanford arm's terminal\n");
  335.         exit(5);
  336.     }
  337. #endif
  338.     userfn1 = fn1;
  339.     userfn2 = fn2;
  340.     drcROBOT.R_routine = onintr1;
  341.     drcROBOT.R_routine2 = onintr2;
  342.  
  343.  (void) signal(SIGINT, release);
  344.  (void) signal(SIGHUP, ondrcerr);
  345.  
  346.     terminate = NO;
  347.     active = YES;
  348.     hang = YES;
  349.     mess = "";
  350.     exch = 0;
  351.     ntest = NTEST;
  352.     rate = (1 << DEFAULTRATE) * HARDCLOCK;
  353.     checking = Magic_Circus;
  354.  
  355.     for (count = 0; count < NJOINTS; ++count) {
  356.         chg.i_motion[count].set = NO;
  357.     }
  358.     chg.a_motion.set = NO;
  359.     chg.g_adco.set = NO;
  360.     chg.g_hand.set = NO;
  361.     chg.g_rate.set = NO;
  362.     chg.end = NO;
  363.     chg.stop = NO;
  364.     chg.calib = NO;
  365. #ifdef PUMA
  366.     if ((fdr = open("/dev/drc0", 2)) < 0) {
  367.         printf("Can't open /dev/drc0 for write\n");
  368.         exit(1);
  369.     }
  370. #endif
  371. #ifdef STAN
  372.     {
  373.         printf("Interface not implemented\n");
  374.         exit(1);
  375.     }
  376. #endif
  377.     if ((count = write(fdr, (char *)&drcROBOT,
  378.      sizeof (struct drcROBOT))) != sizeof (struct drcROBOT)) {
  379.         printf("write error initing drcROBOT, count:%d\n", count);
  380.         exit(2);
  381.     }
  382.     printf("** channel opened, turn on ARM POWER\n");
  383.     while (hang) {
  384.         nap(10);
  385.     }
  386. }
  387.  
  388. adcopen(ch) /*::*/
  389. int ch;
  390. {
  391. #ifdef ADC
  392.     if (!active || ch < NJOINTS || ch >= ADC || adcmap[ch] >= 0) {
  393.         return(-1);
  394.     }
  395.     chg.g_adco.set = YES;
  396.     chg.g_adco.valg = ch;
  397.     while (chg.g_adco.set) {
  398.         nap(1);
  399.     }
  400.     return(adcmap[ch] = nch - 1);
  401. #else
  402.     return(-1);
  403. #endif
  404. }
  405.  
  406.  
  407.  
  408.  
  409. static onintr1() /*##*/
  410. {
  411.     register short chks;
  412.     register short *sp;
  413.     register int i;
  414.     register short inc;
  415.     static short old[NJOINTS];
  416.  
  417.     if (inserf) {
  418.         mess = "** interrupt occurred before end of user function";
  419.         terminate = TOOF;
  420.         return;
  421.     }
  422.     inserf = YES;
  423.     if (terminate == TOOF || terminate == BAD) {
  424.         inserf = NO;
  425.         return;
  426.     }
  427.     if (ntest) {
  428.         --ntest;
  429.         for (i = 0; i < FIFOBUFS; ++i) {
  430.             cmdbuf.data[i] = OD;
  431.             if (how.howb.data[i] != (short)ID) {
  432.                 cmdbuf.data[0] = 0;
  433.                 mess = "**** hardware failure";
  434.                 terminate = BAD;
  435.                 return;
  436.             }
  437.         }
  438.         cmdbuf.wc = FIFOBUFS;
  439.         hang = YES;
  440.         return;
  441.     }
  442.     sp = (short *)how.howw.pos;
  443.     chks = how.howw.exio;
  444.     for (i = 0; i < NJOINTS; ++i) {
  445.         chks += *sp++;
  446.     }
  447. #ifdef ADC
  448.     for (i = 0; i < nch; ++i) {
  449.         chks += *sp++;
  450.     }
  451. #endif
  452.     if(*sp != chks) {
  453.         terminate = BAD;
  454.         mess = "** bad checksum";
  455.         return;
  456.     }
  457.     if (checking) {
  458. #ifdef ADC
  459.         for (i = 0; i < NJOINTS; ++i) {
  460.             inc = how.howw.adcr[i];
  461.             inc = (inc < 0) ? -inc : inc;
  462.             if (inc > mxoc[i]) {
  463.                 mess = "** too large observed current";
  464.                 terminate = YES;
  465.                 break;
  466.             }
  467.         }
  468. #endif
  469.         if (exch == 0) {
  470.             for (i = 0; i < NJOINTS; ++i) {
  471.                 old[i] = how.howw.pos[i];
  472.             }
  473.         }
  474.         else {
  475.             for (i = 0; i < NJOINTS; ++i) {
  476.                 inc = how.howw.pos[i] - old[i];
  477.                 inc = (inc < 0) ? -inc : inc;
  478.                 if (inc > mvl[i] * rate) {
  479.                     mess = "** too large observed velocity";
  480.                     terminate = YES;
  481.                     break;
  482.                 }
  483.                 inc = old[i] = how.howw.pos[i];
  484.                 if (inc > max[i] || inc < min[i]) {
  485.                     mess = "** joint(s) out of range";
  486.                     terminate = YES;
  487.                     break;
  488.                 }
  489.             }
  490.         }
  491.     }
  492.     if (exch != 0) {
  493.         if (!(terminate || (hang = !(how.howw.exio & ARMPWR)))) {
  494.             (* userfn1)();  /* process arm state */
  495.         }
  496.         i = cmdbuf.wc = pack();     /* word count */
  497.         chks = 0;
  498.         for (sp = cmdbuf.data; i--;)
  499.             chks += *sp++;
  500.         *sp = chks;
  501.         ++cmdbuf.wc;                /* for chks   */
  502.     }
  503.     else {
  504.         cmdbuf.wc = 2 + 2 * NJOINTS;
  505.         cmdbuf.data[0] = VERSION;
  506.         cmdbuf.data[1] = ARMTYPE;
  507.         for (i = 0; i < NJOINTS; ++i) {
  508.             cmdbuf.data[i + 2] = ref[i];
  509.             cmdbuf.data[i + 2 + NJOINTS] = idx[i];
  510.         }
  511.     }
  512.     ++exch;
  513. }
  514.  
  515.  
  516.  
  517. static onintr2() /*##*/
  518. {
  519.     if (terminate) {
  520.         inserf = NO;
  521.         return;
  522.     }
  523.     if (!hang) {
  524.         (* userfn2)();  /* command arm */
  525.     }
  526.     inserf = NO;
  527. }
  528.  
  529.  
  530. static ondrcerr() /*##*/
  531. {
  532.  (void) close(fdr);
  533.     printf("**** driver error...exch = %d\n", exch);
  534.     printf("\nterminate %d\n", terminate);
  535.     exit(3);
  536. }
  537.  
  538.  
  539. release(s)
  540. char *s;
  541. {
  542.     int c;
  543.  
  544.     terminate = YES;
  545.     active = NO;
  546.     nap(10);
  547. #ifdef ADC
  548.     nch = NJOINTS;
  549.     for (c = NJOINTS; c < ADC; ++c) {
  550.         adcmap[c] = -1;
  551.     }
  552. #endif
  553.  (void) close(fdr);
  554.     if ((int)s == SIGINT) {
  555.         printf("\n** Interrupted\n");
  556.         printf("%s\n", mess);
  557.     }
  558.     else {
  559.         printf("%s\n", s);
  560.         printf("%s\n", mess);
  561.     }
  562.     printf("** exch = %d\n", exch);
  563.  
  564.     if ((int)s == SIGINT) {
  565.         if (cbrkset) {
  566.             mode.sg_flags &= ~CBREAK;
  567.             ioctl(1, TIOCSETP, &mode);
  568.         }
  569.         printf("** attempt an automatic home return ? ");
  570.         QUERY(c);
  571.         if (c == 'y') {
  572.             home();
  573.         }
  574.         exit(1);
  575.     }
  576. }
  577.  
  578.  
  579. /*
  580.  * manual stepping mode
  581.  */
  582.  
  583. #define GO(x ,y) printf("%c=%c%c" ,'\033' ,' '+x ,' '+y)
  584.  
  585. #define CALREQ  2
  586. #define BS      '\010'
  587. #define CR      '\015'
  588. #define MAXMAG 100
  589. #define HANDSTEP 10
  590.  
  591. static int enough = NO;
  592. static int mag = 0;
  593. static int poshd = 0;
  594. static unsigned short ideal[NJOINTS];
  595. static int offset[NJOINTS] = {0, 0, 0, 0, 0, 0};
  596.  
  597.  
  598.  
  599. stepmode() /*::*/
  600. {
  601.     int dummy(), soft();
  602.     int c;
  603.     int sig = YES;
  604.  
  605.     enough = NO;
  606.     first = YES;
  607.     Magic_Circus = NO;
  608.     mag = 0;
  609.     poshd = 0;
  610.     for (c = 0; c < NJOINTS; ++c) {
  611.         offset[c] = 0;
  612.     }
  613.  
  614.     if (gtty(1, &mode) < 0) {
  615.         printf("** can't gtty\n");
  616.         exit(1);
  617.     }
  618.     control(dummy, soft);
  619.     while(first) {
  620.         nap(10);
  621.     }
  622.     mode.sg_flags |= CBREAK;
  623.     ioctl(1, TIOCSETP, &mode);
  624.     cbrkset = YES;
  625.     GO(22, 0);
  626.     printf(
  627. "faster(.) slower(,) reverse(-) joints[1-6] hand(o/c) exit(return)\n\n");
  628.     printf(
  629. "** exit ? (y/n) _  calibrate ? (y/n) _  sure ? (y/n) _");
  630.     GO(22, 0);
  631.     printf("*     ");
  632.     for (c = 0; c < NJOINTS; ++c) {
  633.         GO(22, ((c + 1) * 8));
  634.         printf("%6u", how.howw.pos[c]);
  635.     }
  636.     while (!enough) {
  637.         putchar(CR);
  638.         putchar(':');
  639.         c = getchar();
  640.         putchar(BS);
  641.         switch (c) {
  642.         case '1' :
  643.         case '2' :
  644.         case '3' :
  645.         case '4' :
  646.         case '5' :
  647.         case '6' :
  648.             offset[c -= '1'] += (sig) ? mag : -mag;
  649.             GO(22, ((c + 1) * 8));
  650.             printf("%6u", how.howw.pos[c]);
  651.             break;
  652. #ifdef PUMA
  653.         case 'o' :
  654.             poshd = 'o';
  655.             break;
  656.         case 'c' :
  657.             poshd = 'c';
  658.             break;
  659. #endif
  660. #ifdef STAN
  661.         case 'o' :
  662.             poshd += HANDSTEP;
  663.             break;
  664.         case 'c' :
  665.             poshd -= HANDSTEP;
  666.             break;
  667. #endif
  668.         case '.' :
  669.             (mag < MAXMAG) ? ++mag : mag;
  670.             break;
  671.         case ',' :
  672.             (mag) ? --mag : mag;
  673.             break;
  674.         case '-' :
  675.             sig = !sig;
  676.             break;
  677.         case '\n' :
  678.             printf("** exit ? (y/n) ");
  679.             if (getchar() == 'y') {
  680.                 printf("  calibrate ? (y/n) ");
  681.                 if (getchar() == 'y') {
  682.                     printf("  sure ? (y/n) ");
  683.                     if (getchar() == 'y') {
  684.                         chg.calib = YES;
  685.                         enough = CALREQ;
  686.                         nap(20);
  687.                     }
  688.                     else {
  689.                         GO(22, 0);
  690.                     }
  691.                 }
  692.                 else {
  693.                     printf("  sure ? (y/n) ");
  694.                     if (getchar() == 'y') {
  695.                         enough = YES;
  696.                     }
  697.                     else {
  698.                         GO(22, 0);
  699.                     }
  700.                 }
  701.             }
  702.             else {
  703.                 GO(22, 1);
  704.             }
  705.             break;
  706.         default :
  707.             break;
  708.         }
  709.     }
  710.     mode.sg_flags &= ~CBREAK;
  711.     ioctl(1, TIOCSETP, &mode);
  712.     cbrkset = NO;
  713.     release("\n** end of step mode");
  714.     Magic_Circus = YES;
  715.     if (enough == CALREQ) {
  716.         printf("** calibrating\n");
  717.         sleep(5);
  718.     }
  719. }
  720.  
  721.  
  722.  
  723. static soft() /*##*/
  724. {
  725.     register int i;
  726.  
  727.     if (enough) {
  728.         return;
  729.     }
  730.     if (first) {
  731.         first = NO;
  732.         for (i = 0; i < NJOINTS; ++i) {
  733.             ideal[i] = how.howw.pos[i];
  734.         }
  735.     }
  736.     for (i = 0; i < NJOINTS; ++i) {
  737.         if (how.howw.pos[i] <= MAXMAG) {
  738.             ideal[i] = chg.i_motion[i].vali = 077777;
  739.             chg.i_motion[i].set = STOPCAL;
  740.         }
  741.         if (how.howw.pos[i] >= 65535-MAXMAG) {
  742.             ideal[i] = chg.i_motion[i].vali = 077777;
  743.             chg.i_motion[i].set = STOPCAL;
  744.         }
  745.         if (offset[i] > 0) {
  746.             offset[i] -= mag;
  747.             ideal[i] -= mag;
  748.         }
  749.         if (offset[i] < 0) {
  750.             offset[i] += mag;
  751.             ideal[i] += mag;
  752.         }
  753.         chg.a_motion.vala[i] = ideal[i];
  754.     }
  755.     chg.a_motion.set = POS;
  756.     chg.g_hand.valg = poshd;
  757.     chg.g_hand.set = YES;
  758. }
  759.  
  760.  
  761. /*
  762.  * Automatic home position return
  763.  */
  764.  
  765. #define SMALL   100
  766. #define STABTIM 50
  767.  
  768. static home() /*##*/
  769. {
  770.     int dummy(), back();
  771.  
  772.     first = YES;
  773.     Magic_Circus = NO;
  774.     printf("** attempting a home return\n");
  775.     control(dummy, back);
  776.     while (!terminate) {
  777.         nap(10);
  778.     }
  779.     nap(10);
  780.  (void) close(fdr);
  781.     printf("** back home, calibrated\n");
  782.     Magic_Circus = YES;
  783.     nap(10);
  784. }
  785.  
  786.  
  787.  
  788. static back() /*##*/
  789. {
  790.     static int finish;
  791.     static int count;
  792.     static unsigned short   where[NJOINTS],
  793.                 inc[NJOINTS],
  794.                 half[NJOINTS],
  795.                 done[NJOINTS];
  796.     register int i;
  797.  
  798.     if (first) {
  799.         first = NO;
  800.         for (i = 0; i < NJOINTS; ++i) {
  801.             where[i] = how.howw.pos[i];
  802.             if (where[i] > ref[i]) {
  803.                 half[i] = (where[i] - ref[i]) / 2 + ref[i];
  804.                 half[i] -= SMALL;
  805.             }
  806.             else {
  807.                 half[i] = ref[i] - (ref[i] - where[i]) / 2;
  808.                 half[i] += SMALL;
  809.             }
  810.             finish = NO;
  811.             count = STABTIM;
  812.             inc[i] = 0;
  813.         }
  814.     }
  815.     if (!finish) {
  816.         for (i = 0, finish = YES; i < NJOINTS; ++i) {
  817.             if (where[i] > ref[i]) {
  818.                 done[i] = where[i] - ref[i] < SMALL;
  819.             }
  820.             else {
  821.                 done[i] = ref[i] - where[i] < SMALL;
  822.             }
  823.             if (!done[i]) {
  824.                 finish = NO;
  825.             }
  826.         }
  827.     }
  828.     else {
  829.         if (--count == 0) {
  830.             terminate = YES;
  831.         }
  832.     }
  833.     for (i = 0; i < NJOINTS; ++i) {
  834.         if (!done[i]) {
  835.             if (where[i] > ref[i]) {
  836.                 if (where[i] > half[i]) {
  837.                     --inc[i];
  838.                 }
  839.                 else {
  840.                     ++inc[i];
  841.                 }
  842.             }
  843.             if (where[i] < ref[i]) {
  844.                 if (where[i] < half[i]) {
  845.                     ++inc[i];
  846.                 }
  847.                 else {
  848.                     --inc[i];
  849.                 }
  850.             }
  851.         }
  852.         else {
  853.             where[i] = ref[i];
  854.         }
  855.         where[i] += inc[i];
  856.         chg.a_motion.vala[i] = where[i];
  857.     }
  858.     chg.a_motion.set = POS;
  859. }
  860.