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