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