home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL071 < prev    next >
Encoding:
Text File  |  1987-03-02  |  5.0 KB  |  321 lines
  1. #include "../h/which.h"
  2. #include "../h/addefs.h"
  3. #include "../h/jcom.h"
  4. #include "../h/unim.h"
  5. #include "../h/rtc.h"
  6. #include "../h/adc.h"
  7.  
  8. #define HNDPROC         6
  9.  
  10. static short ibuff[COMSIZE];    /* input buffer */
  11. static struct how how;          /* output buffer */
  12. static short ref[NJOINTS];
  13. static short idx[NJOINTS];
  14. static short calreq;
  15. static short first;
  16. #ifdef ADC
  17. static short adcmap[ADC];
  18. static short nch;
  19. #endif
  20. #ifdef VALII
  21. static short rate[6] = {2, 4, 8, 16, 32, 64};
  22. #else
  23. static short rate[6] = {1, 2, 4, 8, 16, 32};
  24. #endif
  25. static short ninter[6] = {8, 16, 32, 64, 128, 256};
  26. static short intscl[6] = {5, 4, 3, 2, 1, 0};
  27.  
  28. #define GETSHORT        value = *scan++; value <<= 8; value |= *scan++ & 0xff;
  29.  
  30. short drive()
  31. {
  32.     register char *scan = (char *)ibuff;
  33.     register unsigned short value = 0;
  34.     register short i = 0;
  35.     unsigned short chks = 0;
  36.     short wc, dr11();
  37.     short *sp;
  38.     unsigned short set[6];
  39.  
  40.     extern short tick;
  41.  
  42. /*
  43.  * gather data
  44.  */
  45.  
  46.     how.exio = getio(-1);
  47.     get6j(READ, how.pos);
  48. #ifdef ADC
  49.     for (i = 0; i < ADC; ++i) {
  50.         if (adcmap[i] >= 0) {
  51.             ADCV(how.adcr[adcmap[i]], i);
  52.         }
  53.     }
  54. #endif
  55.     sp = (short *)how.pos;
  56.     chks = how.exio;
  57.     for (i = 0; i < NJOINTS; ++i) {
  58.          chks += *sp++;
  59.     }
  60. #ifdef ADC
  61.     for (i = 0; i < nch; ++i) {
  62.          chks += *sp++;
  63.     }
  64. #endif
  65.     *sp = chks;
  66. /*
  67.  * interrupt vax send data, get commands
  68.  */
  69. #ifdef ADC
  70.     if ((wc = dr11(ibuff, &how, 1 + NJOINTS + nch + 1)) < 0) {
  71.         zexit(wc);
  72.     }
  73. #else
  74.     if ((wc = dr11(ibuff, &how, 1 + NJOINTS + 1)) < 0) {
  75.         zexit(wc);
  76.     }
  77. #endif
  78.     if (wc == 0) {
  79.         return;
  80.     }
  81. /*
  82.  * analyze commands and operate the arm
  83.  */
  84.     if (first) {
  85.         first = NO;
  86.         if (ibuff[0] != VERSION)
  87.             zexit(13);
  88.         if (ibuff[1] != ARMTYPE)
  89.             zexit(14);
  90. #ifdef STAN
  91.         putp(MODE, MESERVO, 6);
  92. #endif
  93.         for (i = 0; i < NJOINTS; ++i) {
  94.             ref[i] = ibuff[i + 2];
  95.             idx[i] = ibuff[i + 2 + NJOINTS];
  96.         }
  97.         put6j(STOPP, /* dummy arg to satisfy macro */ set);
  98.         put6pg(MODE, MESERVO);
  99. #ifdef PUMA
  100.         putp(MODE, MESERVO | MEHLFCT, 5);
  101. #endif
  102.         return;
  103.     }
  104.     i = wc - 1;
  105.     sp = ibuff;
  106.     chks = 0;
  107.     while(i--) {
  108.         chks += *sp++;
  109.     }
  110.     if (*sp != chks) {
  111.         zexit(12);
  112.     }
  113.     *sp = -1;
  114.  
  115.     scan = (char *)ibuff;
  116.  
  117.     while (*scan != -1) {
  118.  
  119.         if ((i = (chks = *scan) & 0x0f) <= CMD_I) {
  120.             ++scan;
  121.             GETSHORT
  122.  
  123.             switch (chks & 0x0f0) {
  124.             case STOPP_I :
  125.                 putj(STOPP, value, i);
  126.                 break;
  127.  
  128.             case STOPM_I :
  129.                 putj(STOPM, value, i);
  130.                 break;
  131.  
  132.             case POS_I :
  133.                 putj(POSET, value, i);
  134.                 break;
  135.  
  136.             case CUR_I :
  137.                 putj(DACSET, value, i);
  138.                 break;
  139.  
  140.             default :
  141.                 zexit(15);
  142.             }
  143.             continue;
  144.         }
  145.  
  146.         switch ((chks = *scan) & 0x0f) {
  147.  
  148.         case CMD_A :
  149.             scan++;
  150.             GETSHORT
  151.             set[0] = value;
  152.             GETSHORT
  153.             set[1] = value;
  154.             GETSHORT
  155.             set[2] = value;
  156.             GETSHORT
  157.             set[3] = value;
  158.             GETSHORT
  159.             set[4] = value;
  160.             GETSHORT
  161.             set[5] = value;
  162.  
  163.             switch (chks & 0xff) {
  164.  
  165.             case STOPP_A :
  166.                 put6j(STOPP, set);
  167.                 break;
  168.  
  169.             case STOPM_A :
  170.                 put6j(STOPM, set);
  171.                 break;
  172.  
  173.             case POS_A :
  174.                 put6j(POSET, set);
  175.                 break;
  176.  
  177.             case CUR_A :
  178.                 put6j(DACSET, set);
  179.                 break;
  180.  
  181.             default :
  182.                 zexit(16);
  183.             }
  184.             break;
  185.  
  186.         case CMD_G :
  187.             ++scan;
  188.             GETSHORT
  189.             switch (chks & 0xff) {
  190.  
  191. #ifdef ADC
  192.             case ADCO_G :
  193.                 adcmap[value] = nch++;
  194.                 break;
  195. #endif
  196.  
  197.             case HAND_G :
  198. #ifdef PUMA
  199.                 if(value == 'o') {
  200.                     bicio(HNDOH);
  201.                 }
  202.                 if (value == 'c') {
  203.                     bisio(HNDOH);
  204.                 }
  205. #endif
  206. #ifdef STAN
  207.                 putj(DACSET, value, HNDPROC);
  208. #endif
  209.                 break;
  210.  
  211.             case RATE_G :
  212.                 if (value < 6) {
  213.                     tick = rate[value];
  214.                     put6pg(NINTER, ninter[value]);
  215.                     put6pg(INTSCL, intscl[value]);
  216.                 }
  217.                 break;
  218.  
  219.             default :
  220.                 zexit(17);
  221.             }
  222.  
  223.             break;
  224.  
  225.         case CMD_E :
  226.             switch (*scan & 0xff) {
  227.  
  228.             case STOP_E :
  229.                 put6jg(STOPP, 0);
  230.                 break;
  231.  
  232.             case CALIB_E :
  233.                 calreq = YES;
  234.                 break;
  235.  
  236.             case END_E :
  237.                 tick = 0;
  238.                 break;
  239.  
  240.             default :
  241.                 zexit(18);
  242.             }
  243.             scan += 1;
  244.             break;
  245.  
  246.         default :
  247.             zexit(19);
  248.         }
  249.     }
  250. }
  251.  
  252.  
  253. #ifdef DEBUG
  254. #include "../h/stdio11.h"
  255. #endif
  256.  
  257. main()
  258. {
  259.     extern short (* onclk)();
  260.     extern short tick;
  261.     extern short drive();
  262.     int i;
  263.  
  264. #ifdef DEBUG
  265.     setbuf(stdin, NULL);
  266.     setbuf(stdout, NULL);
  267. #endif
  268.     for (; ; ) {
  269.         first = YES;
  270.         calreq = NO;
  271. #ifdef ADC
  272.         for (i = 0; i < NJOINTS; ++i) {
  273.             adcmap[i] = i;
  274.         }
  275.         for (i = NJOINTS; i < ADC; ++i) {
  276.             adcmap[i] = -1;
  277.         }
  278.         nch = NJOINTS;
  279. #endif
  280.         bicio(ARMPWR);
  281.         bisio(ARMPWR);
  282.         put6pg(MODE, MESERVO|MEPOSSV);
  283.         put6pg(NINTER, ninter[DEFAULTRATE]);
  284.         put6pg(INTSCL, intscl[DEFAULTRATE]);
  285.         while (!getio(ARMPWR))
  286.             ;
  287. #ifdef DEBUG
  288.         printf("active\n");
  289. #endif
  290.         onclk = drive;
  291.         tick = rate[DEFAULTRATE];
  292.         while (tick)
  293.             wait();
  294. #ifdef DEBUG
  295.         printf("iddle\n");
  296. #endif
  297.         if (calreq) {
  298.             if (!cal(ref, idx)) {
  299.                 zexit(99);
  300.             }
  301.         }
  302.         else {
  303.             bicio(ARMPWR);
  304.             for (i = 50000; --i; )
  305.                 ;
  306.         }
  307.     }
  308. }
  309.  
  310.  
  311. zexit(n)
  312. {
  313.     tick = 0;
  314.     *VAXCSR = 0;
  315.     bicio(ARMPWR);
  316. #ifdef DEBUG
  317.     printf("\nexit %d\n", n);
  318. #endif
  319.     halt();
  320. }
  321.