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