home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL097 < prev    next >
Encoding:
Text File  |  1987-03-02  |  17.2 KB  |  862 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.     /*
  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.     */
  340.     userfn1 = fn1;
  341.     userfn2 = fn2;
  342.     drcROBOT.R_routine = onintr1;
  343.     drcROBOT.R_routine2 = onintr2;
  344.  
  345.  (void) signal(SIGINT, release);
  346.  (void) signal(SIGHUP, ondrcerr);
  347.  
  348.     terminate = NO;
  349.     active = YES;
  350.     hang = YES;
  351.     mess = "";
  352.     exch = 0;
  353.     ntest = NTEST;
  354.     rate = (1 << DEFAULTRATE) * HARDCLOCK;
  355.     checking = Magic_Circus;
  356.  
  357.     for (count = 0; count < NJOINTS; ++count) {
  358.         chg.i_motion[count].set = NO;
  359.     }
  360.     chg.a_motion.set = NO;
  361.     chg.g_adco.set = NO;
  362.     chg.g_hand.set = NO;
  363.     chg.g_rate.set = NO;
  364.     chg.end = NO;
  365.     chg.stop = NO;
  366.     chg.calib = NO;
  367. #ifdef PUMA
  368.     if ((fdr = open("/dev/drc0", 2)) < 0) {
  369.         printf("Can't open /dev/drc0 for write\n");
  370.         exit(1);
  371.     }
  372. #endif
  373. #ifdef STAN
  374.     {
  375.         printf("Interface not implemented\n");
  376.         exit(1);
  377.     }
  378. #endif
  379.     if ((count = write(fdr, (char *)&drcROBOT,
  380.      sizeof (struct drcROBOT))) != sizeof (struct drcROBOT)) {
  381.         printf("write error initing drcROBOT, count:%d\n", count);
  382.         exit(2);
  383.     }
  384.     printf("** channel opened, turn on ARM POWER\n");
  385.     while (hang) {
  386.         nap(10);
  387.     }
  388. }
  389.  
  390. adcopen(ch) /*::*/
  391. int ch;
  392. {
  393. #ifdef ADC
  394.     if (!active || ch < NJOINTS || ch >= ADC || adcmap[ch] >= 0) {
  395.         return(-1);
  396.     }
  397.     chg.g_adco.set = YES;
  398.     chg.g_adco.valg = ch;
  399.     while (chg.g_adco.set) {
  400.         nap(1);
  401.     }
  402.     return(adcmap[ch] = nch - 1);
  403. #else
  404.     return(-1);
  405. #endif
  406. }
  407.  
  408.  
  409.  
  410.  
  411. static onintr1() /*##*/
  412. {
  413.     register short chks;
  414.     register short *sp;
  415.     register int i;
  416.     register short inc;
  417.     static short old[NJOINTS];
  418.  
  419.     if (inserf) {
  420.         mess = "** interrupt occurred before end of user function";
  421.         terminate = TOOF;
  422.         return;
  423.     }
  424.     inserf = YES;
  425.     if (terminate == TOOF || terminate == BAD) {
  426.         inserf = NO;
  427.         return;
  428.     }
  429.     if (ntest) {
  430.         --ntest;
  431.         for (i = 0; i < FIFOBUFS; ++i) {
  432.             cmdbuf.data[i] = OD;
  433.             if (how.howb.data[i] != (short)ID) {
  434.                 cmdbuf.data[0] = 0;
  435.                 mess = "**** hardware failure";
  436.                 terminate = BAD;
  437.                 return;
  438.             }
  439.         }
  440.         cmdbuf.wc = FIFOBUFS;
  441.         hang = YES;
  442.         return;
  443.     }
  444.     sp = (short *)how.howw.pos;
  445.     chks = how.howw.exio;
  446.     for (i = 0; i < NJOINTS; ++i) {
  447.         chks += *sp++;
  448.     }
  449. #ifdef ADC
  450.     for (i = 0; i < nch; ++i) {
  451.         chks += *sp++;
  452.     }
  453. #endif
  454.     if(*sp != chks) {
  455.         terminate = BAD;
  456.         mess = "** bad checksum";
  457.         return;
  458.     }
  459.     if (checking) {
  460. #ifdef ADC
  461.         for (i = 0; i < NJOINTS; ++i) {
  462.             inc = how.howw.adcr[i];
  463.             inc = (inc < 0) ? -inc : inc;
  464.             if (inc > mxoc[i]) {
  465.                 mess = "** too large observed current";
  466.                 terminate = YES;
  467.                 break;
  468.             }
  469.         }
  470. #endif
  471.         if (exch == 0) {
  472.             for (i = 0; i < NJOINTS; ++i) {
  473.                 old[i] = how.howw.pos[i];
  474.             }
  475.         }
  476.         else {
  477.             for (i = 0; i < NJOINTS; ++i) {
  478.                 inc = how.howw.pos[i] - old[i];
  479.                 inc = (inc < 0) ? -inc : inc;
  480.                 if (inc > mvl[i] * rate) {
  481.                     mess = "** too large observed velocity";
  482.                     terminate = YES;
  483.                     break;
  484.                 }
  485.                 inc = old[i] = how.howw.pos[i];
  486.                 if (inc > max[i] || inc < min[i]) {
  487.                     mess = "** joint(s) out of range";
  488.                     terminate = YES;
  489.                     break;
  490.                 }
  491.             }
  492.         }
  493.     }
  494.     if (exch != 0) {
  495.         if (!(terminate || (hang = !(how.howw.exio & ARMPWR)))) {
  496.             (* userfn1)();  /* process arm state */
  497.         }
  498.         i = cmdbuf.wc = pack();     /* word count */
  499.         chks = 0;
  500.         for (sp = cmdbuf.data; i--;)
  501.             chks += *sp++;
  502.         *sp = chks;
  503.         ++cmdbuf.wc;                /* for chks   */
  504.     }
  505.     else {
  506.         cmdbuf.wc = 2 + 2 * NJOINTS;
  507.         cmdbuf.data[0] = VERSION;
  508.         cmdbuf.data[1] = ARMTYPE;
  509.         for (i = 0; i < NJOINTS; ++i) {
  510.             cmdbuf.data[i + 2] = ref[i];
  511.             cmdbuf.data[i + 2 + NJOINTS] = idx[i];
  512.         }
  513.     }
  514.     ++exch;
  515. }
  516.  
  517.  
  518.  
  519. static onintr2() /*##*/
  520. {
  521.     if (terminate) {
  522.         inserf = NO;
  523.         return;
  524.     }
  525.     if (!hang) {
  526.         (* userfn2)();  /* command arm */
  527.     }
  528.     inserf = NO;
  529. }
  530.  
  531.  
  532. static ondrcerr() /*##*/
  533. {
  534.  (void) close(fdr);
  535.     printf("**** driver error...exch = %d\n", exch);
  536.     printf("\nterminate %d\n", terminate);
  537.     exit(3);
  538. }
  539.  
  540.  
  541. release(s)
  542. char *s;
  543. {
  544.     int c;
  545.  
  546.     terminate = YES;
  547.     active = NO;
  548.     nap(10);
  549. #ifdef ADC
  550.     nch = NJOINTS;
  551.     for (c = NJOINTS; c < ADC; ++c) {
  552.         adcmap[c] = -1;
  553.     }
  554. #endif
  555.  (void) close(fdr);
  556.     if ((int)s == SIGINT) {
  557.         printf("\n** Interrupted\n");
  558.         printf("%s\n", mess);
  559.     }
  560.     else {
  561.         printf("%s\n", s);
  562.         printf("%s\n", mess);
  563.     }
  564.     printf("** exch = %d\n", exch);
  565.  
  566.     if ((int)s == SIGINT) {
  567.         if (cbrkset) {
  568.             mode.sg_flags &= ~CBREAK;
  569.             ioctl(1, TIOCSETP, &mode);
  570.         }
  571.         printf("** attempt an automatic home return ? ");
  572.         QUERY(c);
  573.         if (c == 'y') {
  574.             home();
  575.         }
  576.         exit(1);
  577.     }
  578. }
  579.  
  580.  
  581. /*
  582.  * manual stepping mode
  583.  */
  584.  
  585. #define GO(x ,y) printf("%c=%c%c" ,'\033' ,' '+x ,' '+y)
  586.  
  587. #define CALREQ  2
  588. #define BS      '\010'
  589. #define CR      '\015'
  590. #define MAXMAG 100
  591. #define HANDSTEP 10
  592.  
  593. static int enough = NO;
  594. static int mag = 0;
  595. static int poshd = 0;
  596. static unsigned short ideal[NJOINTS];
  597. static int offset[NJOINTS] = {0, 0, 0, 0, 0, 0};
  598.  
  599.  
  600.  
  601. stepmode() /*::*/
  602. {
  603.     int dummy(), soft();
  604.     int c;
  605.     int sig = YES;
  606.  
  607.     enough = NO;
  608.     first = YES;
  609.     Magic_Circus = NO;
  610.     mag = 0;
  611.     poshd = 0;
  612.     for (c = 0; c < NJOINTS; ++c) {
  613.         offset[c] = 0;
  614.     }
  615.  
  616.     if (gtty(1, &mode) < 0) {
  617.         printf("** can't gtty\n");
  618.         exit(1);
  619.     }
  620.     control(dummy, soft);
  621.     while(first) {
  622.         nap(10);
  623.     }
  624.     mode.sg_flags |= CBREAK;
  625.     ioctl(1, TIOCSETP, &mode);
  626.     cbrkset = YES;
  627.     GO(22, 0);
  628.     printf(
  629. "faster(.) slower(,) reverse(-) joints[1-6] hand(o/c) exit(return)\n\n");
  630.     printf(
  631. "** exit ? (y/n) _  calibrate ? (y/n) _  sure ? (y/n) _");
  632.     GO(22, 0);
  633.     printf("*     ");
  634.     for (c = 0; c < NJOINTS; ++c) {
  635.         GO(22, ((c + 1) * 8));
  636.         printf("%6u", how.howw.pos[c]);
  637.     }
  638.     while (!enough) {
  639.         putchar(CR);
  640.         putchar(':');
  641.         c = getchar();
  642.         putchar(BS);
  643.         switch (c) {
  644.         case '1' :
  645.         case '2' :
  646.         case '3' :
  647.         case '4' :
  648.         case '5' :
  649.         case '6' :
  650.             offset[c -= '1'] += (sig) ? mag : -mag;
  651.             GO(22, ((c + 1) * 8));
  652.             printf("%6u", how.howw.pos[c]);
  653.             break;
  654. #ifdef PUMA
  655.         case 'o' :
  656.             poshd = 'o';
  657.             break;
  658.         case 'c' :
  659.             poshd = 'c';
  660.             break;
  661. #endif
  662. #ifdef STAN
  663.         case 'o' :
  664.             poshd += HANDSTEP;
  665.             break;
  666.         case 'c' :
  667.             poshd -= HANDSTEP;
  668.             break;
  669. #endif
  670.         case '.' :
  671.             (mag < MAXMAG) ? ++mag : mag;
  672.             break;
  673.         case ',' :
  674.             (mag) ? --mag : mag;
  675.             break;
  676.         case '-' :
  677.             sig = !sig;
  678.             break;
  679.         case '\n' :
  680.             printf("** exit ? (y/n) ");
  681.             if (getchar() == 'y') {
  682.                 printf("  calibrate ? (y/n) ");
  683.                 if (getchar() == 'y') {
  684.                     printf("  sure ? (y/n) ");
  685.                     if (getchar() == 'y') {
  686.                         chg.calib = YES;
  687.                         enough = CALREQ;
  688.                         nap(20);
  689.                     }
  690.                     else {
  691.                         GO(22, 0);
  692.                     }
  693.                 }
  694.                 else {
  695.                     printf("  sure ? (y/n) ");
  696.                     if (getchar() == 'y') {
  697.                         enough = YES;
  698.                     }
  699.                     else {
  700.                         GO(22, 0);
  701.                     }
  702.                 }
  703.             }
  704.             else {
  705.                 GO(22, 1);
  706.             }
  707.             break;
  708.         default :
  709.             break;
  710.         }
  711.     }
  712.     mode.sg_flags &= ~CBREAK;
  713.     ioctl(1, TIOCSETP, &mode);
  714.     cbrkset = NO;
  715.     release("\n** end of step mode");
  716.     Magic_Circus = YES;
  717.     if (enough == CALREQ) {
  718.         printf("** calibrating\n");
  719.         sleep(5);
  720.     }
  721. }
  722.  
  723.  
  724.  
  725. static soft() /*##*/
  726. {
  727.     register int i;
  728.  
  729.     if (enough) {
  730.         return;
  731.     }
  732.     if (first) {
  733.         first = NO;
  734.         for (i = 0; i < NJOINTS; ++i) {
  735.             ideal[i] = how.howw.pos[i];
  736.         }
  737.     }
  738.     for (i = 0; i < NJOINTS; ++i) {
  739.         if (how.howw.pos[i] <= MAXMAG) {
  740.             ideal[i] = chg.i_motion[i].vali = 077777;
  741.             chg.i_motion[i].set = STOPCAL;
  742.         }
  743.         if (how.howw.pos[i] >= 65535-MAXMAG) {
  744.             ideal[i] = chg.i_motion[i].vali = 077777;
  745.             chg.i_motion[i].set = STOPCAL;
  746.         }
  747.         if (offset[i] > 0) {
  748.             offset[i] -= mag;
  749.             ideal[i] -= mag;
  750.         }
  751.         if (offset[i] < 0) {
  752.             offset[i] += mag;
  753.             ideal[i] += mag;
  754.         }
  755.         chg.a_motion.vala[i] = ideal[i];
  756.     }
  757.     chg.a_motion.set = POS;
  758.     chg.g_hand.valg = poshd;
  759.     chg.g_hand.set = YES;
  760. }
  761.  
  762.  
  763. /*
  764.  * Automatic home position return
  765.  */
  766.  
  767. #define SMALL   100
  768. #define STABTIM 50
  769.  
  770. static home() /*##*/
  771. {
  772.     int dummy(), back();
  773.  
  774.     first = YES;
  775.     Magic_Circus = NO;
  776.     printf("** attempting a home return\n");
  777.     control(dummy, back);
  778.     while (!terminate) {
  779.         nap(10);
  780.     }
  781.     nap(10);
  782.  (void) close(fdr);
  783.     printf("** back home, calibrated\n");
  784.     Magic_Circus = YES;
  785.     nap(10);
  786. }
  787.  
  788.  
  789.  
  790. static back() /*##*/
  791. {
  792.     static int finish;
  793.     static int count;
  794.     static unsigned short   where[NJOINTS],
  795.                 inc[NJOINTS],
  796.                 half[NJOINTS],
  797.                 done[NJOINTS];
  798.     register int i;
  799.  
  800.     if (first) {
  801.         first = NO;
  802.         for (i = 0; i < NJOINTS; ++i) {
  803.             where[i] = how.howw.pos[i];
  804.             if (where[i] > ref[i]) {
  805.                 half[i] = (where[i] - ref[i]) / 2 + ref[i];
  806.                 half[i] -= SMALL;
  807.             }
  808.             else {
  809.                 half[i] = ref[i] - (ref[i] - where[i]) / 2;
  810.                 half[i] += SMALL;
  811.             }
  812.             finish = NO;
  813.             count = STABTIM;
  814.             inc[i] = 0;
  815.         }
  816.     }
  817.     if (!finish) {
  818.         for (i = 0, finish = YES; i < NJOINTS; ++i) {
  819.             if (where[i] > ref[i]) {
  820.                 done[i] = where[i] - ref[i] < SMALL;
  821.             }
  822.             else {
  823.                 done[i] = ref[i] - where[i] < SMALL;
  824.             }
  825.             if (!done[i]) {
  826.                 finish = NO;
  827.             }
  828.         }
  829.     }
  830.     else {
  831.         if (--count == 0) {
  832.             terminate = YES;
  833.         }
  834.     }
  835.     for (i = 0; i < NJOINTS; ++i) {
  836.         if (!done[i]) {
  837.             if (where[i] > ref[i]) {
  838.                 if (where[i] > half[i]) {
  839.                     --inc[i];
  840.                 }
  841.                 else {
  842.                     ++inc[i];
  843.                 }
  844.             }
  845.             if (where[i] < ref[i]) {
  846.                 if (where[i] < half[i]) {
  847.                     ++inc[i];
  848.                 }
  849.                 else {
  850.                     --inc[i];
  851.                 }
  852.             }
  853.         }
  854.         else {
  855.             where[i] = ref[i];
  856.         }
  857.         where[i] += inc[i];
  858.         chg.a_motion.vala[i] = where[i];
  859.     }
  860.     chg.a_motion.set = POS;
  861. }
  862.