home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL067 < prev    next >
Encoding:
Text File  |  1987-03-02  |  6.6 KB  |  397 lines
  1. /*
  2.  * MAC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : mac
  6.  *      File    : moper.c
  7.  *      Remarks : superviser  program
  8.  *      Usage   : make install
  9.  *
  10.  *    modified: added asleep external variable see vec.s
  11.  *          for details
  12.  */
  13.  
  14. #include "../h/which.h"
  15. #include "../h/addefs.h"
  16. #include "../h/jcom.h"
  17. #include "../h/cmdk.h"
  18. #include "../h/fifoio.h"
  19. #include "../h/rtc.h"
  20. #include "../h/adc.h"
  21. #include "../h/stdio11.h"
  22.  
  23. #define DEBUG
  24. #define HNDPROC         6
  25.  
  26. #define SMOOTH          8
  27.  
  28. static union howu {
  29.     struct fifobuf howb;
  30.     struct how     howw;
  31. } how;
  32.  
  33. static struct fifobuf cmdbuf;
  34.  
  35. static short calreq;
  36. static short first;
  37. static short ntest;
  38. static short ref[NJOINTS];
  39. static short idx[NJOINTS];
  40. #ifdef ADC
  41. static short adcmap[ADC];
  42. static short nch;
  43. #endif
  44. #ifdef VALII
  45. static short rate[6] = {2, 4, 8, 16, 32, 64};
  46. #else
  47. static short rate[6] = {1, 2, 4, 8, 16, 32};
  48. #endif
  49. static short ninter[6] = {8, 16, 32, 64, 128, 256};
  50. static short intscl[6] = {5, 4, 3, 2, 1, 0};
  51.  
  52. static jst[NJOINTS];
  53. static ldac[NJOINTS];
  54. static ddac[NJOINTS];
  55. int buf;
  56.  
  57. short drive()
  58. {
  59.     register short *scan;
  60.     register short i = 0;
  61.     register unsigned short chks = 0;
  62.     unsigned short set[NJOINTS];
  63.  
  64.     extern short tick;
  65.     extern short asleep;
  66.  
  67.  
  68.     if (ntest) {
  69.         for (i = 0; i < FIFOBUFS; ++i) {
  70.             how.howb.data[i] = ID;
  71.             if (cmdbuf.data[i] != OD) {
  72.                 zexit(10);
  73.             }
  74.         }
  75.         how.howb.wc = FIFOBUFS;
  76.         if ((i = dr11(&cmdbuf, (struct fifobuf *)&how)) < 0) {
  77.             zexit(i - 100);
  78.         }
  79.         --ntest;
  80.         return;
  81.     }
  82. /*
  83.  * gather data
  84.  */
  85.  
  86.     how.howw.exio = getio(-1);
  87.     get6j(READ, how.howw.pos);
  88.  
  89. #ifdef ADC
  90.     for (i = 0; i < ADC; ++i) {
  91.         if (adcmap[i] >= 0) {
  92.             ADCV(how.howw.adcr[adcmap[i]], i);
  93.         }
  94.     }
  95. #endif
  96.     scan = (short *)how.howw.pos;
  97.     chks = how.howw.exio;
  98.     for (i = 0; i < NJOINTS; ++i) {
  99.          chks += *scan++;
  100.     }
  101. #ifdef ADC
  102.     for (i = 0; i < nch; ++i) {
  103.          chks += *scan++;
  104.     }
  105. #endif
  106.     *scan = chks;
  107. /*
  108.  * interrupt vax send data, get commands
  109.  */
  110. #ifdef ADC
  111.     how.howw.wc = 1 + NJOINTS + nch + 1;
  112. #else
  113.     how.howw.wc = 1 + NJOINTS + 1;
  114. #endif
  115.     if ((i = dr11(&cmdbuf, (struct fifobuf *)&how)) < 0) {
  116.         zexit(i);
  117.     }
  118.     if (cmdbuf.wc == 0) {
  119.         return;
  120.     }
  121.  
  122. /*
  123.  * analyze commands and operate the arm
  124.  */
  125.     if (first) {
  126.         first = NO;
  127.         if (cmdbuf.data[0] != VERSION)
  128.             zexit(13);
  129.         if (cmdbuf.data[1] != ARMTYPE)
  130.             zexit(14);
  131.         for (i = 0; i < NJOINTS; ++i) {
  132.             ref[i] = cmdbuf.data[i + 2];
  133.             idx[i] = cmdbuf.data[i + 2 + NJOINTS];
  134.         }
  135. #ifdef PUMA
  136.         putp(MODE, MESERVO | MEHLFCT, 5);
  137. #endif
  138.         return;
  139.     }
  140.     i = cmdbuf.wc - 1;
  141.     scan = cmdbuf.data;
  142.     chks = 0;
  143.     while(i--) {
  144.         chks += *scan++;
  145.     }
  146.     if (*scan != chks) {
  147.         zexit(12);
  148.     }
  149.     *scan = -1;
  150.  
  151.     scan = cmdbuf.data;
  152.  
  153.     while (*scan != -1) {
  154.  
  155.         if ((i = (chks = *scan) & 0x00ff) <= CMD_I) {
  156.             ++scan;
  157.  
  158.             switch (chks & 0x0ff00) {
  159.             case STOPP_I :
  160.                 putj(STOPP, *scan++, i);
  161.                 break;
  162.  
  163.             case STOPM_I :
  164.                 putj(STOPM, *scan++, i);
  165.                 break;
  166.  
  167.             case POS_I :
  168.                 if (jst[i]) {
  169.                     --(jst[i]);
  170.                     putj(GRAVTY, ldac[i], i);
  171.                     ldac[i] -= ddac[i];
  172.                 }
  173.                 putj(POSET, *scan++, i);
  174.                 break;
  175.  
  176.             case CUR_I :
  177.                 ldac[i] = *scan++;
  178.                 putj(DACSET, ldac[i], i);
  179.                 jst[i] = SMOOTH;
  180.                 ldac[i] >>= 1;
  181.                 ddac[i] = ldac[i] >> 3;
  182.                 break;
  183.  
  184.             case GRAV_I :
  185.                 putj(GRAVTY, *scan++, i);
  186.                 break;
  187.  
  188.             default :
  189.                 zexit(15);
  190.             }
  191.             continue;
  192.         }
  193.  
  194.         switch ((chks = *scan) & 0x00ff) {
  195.  
  196.         case CMD_A :
  197.             scan++;
  198.             for (i = 0; i < NJOINTS; ++i) {
  199.                 set[i] = *scan++;
  200.             }
  201.             switch (chks) {
  202.  
  203.             case STOPP_A :
  204.                 put6j(STOPP, set);
  205.                 break;
  206.  
  207.             case STOPM_A :
  208.                 put6j(STOPM, set);
  209.                 break;
  210.  
  211.             case POS_A :
  212.                 put6j(POSET, set);
  213.                 break;
  214.  
  215.             case CUR_A :
  216.                 put6j(DACSET, set);
  217.                 break;
  218.  
  219.             case GRAV_A :
  220.                 put6j(GRAVTY, set);
  221.                 break;
  222.  
  223.             default :
  224.                 zexit(16);
  225.             }
  226.             break;
  227.  
  228.         case CMD_G :
  229.             ++scan;
  230.             switch (chks) {
  231. #ifdef ADC
  232.             case ADCO_G :
  233.                 adcmap[*scan++] = nch++;
  234.                 break;
  235. #endif
  236.  
  237.             case HAND_G :
  238. #ifdef PUMA
  239.                 if(*scan == 'o') {
  240.                     bicio(HNDOH);
  241.                 }
  242.                 if (*scan == 'c') {
  243.                     bisio(HNDOH);
  244.                 }
  245.                 ++scan;
  246. #endif
  247. #ifdef STAN
  248.                 putj(DACSET, *scan++, HNDPROC);
  249. #endif
  250.                 break;
  251.  
  252.             case RATE_G :
  253.                 if (*scan < 6) {
  254.                     tick = rate[*scan];
  255.                     put6pg(NINTER, ninter[*scan]);
  256.                     put6pg(INTSCL, intscl[*scan]);
  257.                 }
  258.                 ++scan;
  259.                 break;
  260.  
  261.             default :
  262.                 zexit(17);
  263.             }
  264.  
  265.             break;
  266.  
  267.         case CMD_E :
  268.             switch (*scan++) {
  269.  
  270.             case STOP_E :
  271.                 put6jg(STOPP, 0);
  272.                 break;
  273.  
  274.             case CALIB_E :
  275.                 calreq = YES;
  276.                 break;
  277.  
  278.             case END_E :
  279.                 tick = 0;
  280.                 break;
  281.  
  282.             default :
  283.                 zexit(18);
  284.             }
  285.             break;
  286.  
  287.         default :
  288.             zexit(19);
  289.         }
  290.     }
  291. }
  292.  
  293.  
  294. #ifdef DEBUG
  295. #include "../h/stdio11.h"
  296. #endif
  297.  
  298. main()
  299. {
  300.     extern short (* onclk)();
  301.     extern short tick;
  302.     extern short asleep;
  303.     extern short drive();
  304.  
  305.     register int i;
  306.     register struct drcdevice *dp = DRC_ADDR;
  307.     short dummy[6];
  308.  
  309. #ifdef DEBUG
  310.     setbuf(stdin, NULL);
  311.     setbuf(stdout, NULL);
  312. #endif
  313.     for (; ; ) {
  314.         first = YES;
  315.         calreq = NO;
  316.         ntest = NTEST;
  317.         for (i = 0; i < FIFOBUFS; ++i) {
  318.             cmdbuf.data[i] = OD;
  319.         }
  320. #ifdef ADC
  321.         for (i = 0; i < NJOINTS; ++i) {
  322.             adcmap[i] = i;
  323.         }
  324.         for (i = NJOINTS; i < ADC; ++i) {
  325.             adcmap[i] = -1;
  326.         }
  327.         nch = NJOINTS;
  328. #endif
  329.         dp->drccsr = 0;
  330.         while ((dp->drccsr & FIFO_EMPTY) == 0) {
  331.             i = dp->drcbuf;
  332.         }
  333.  
  334.         for (i = 0; i < NJOINTS; ++i) {
  335.             jst[i] = 0;
  336.         }
  337.  
  338.         bicio(ARMPWR);
  339.         bisio(ARMPWR);
  340.         put6pg(MODE, MESERVO|MEPOSSV);
  341.         put6pg(NINTER, ninter[DEFAULTRATE]);
  342.         put6pg(INTSCL, intscl[DEFAULTRATE]);
  343. #ifdef STAN
  344.         putp(MODE, MESERVO, 6);
  345. #endif
  346.         put6j(STOPP, dummy);
  347.         put6pg(MODE, MESERVO);
  348. #ifdef PUMA
  349.         putp(MODE, MESERVO | MEHLFCT, 5);
  350. #endif
  351.         while (!getio(ARMPWR)) {
  352.             get6j(READ, how.howw.pos);
  353.         }
  354. #ifdef DEBUG
  355.         printf("active\n");
  356. #endif
  357.         onclk = drive;
  358.         tick = rate[DEFAULTRATE];
  359.         while (tick)
  360.             wait();
  361. #ifdef DEBUG
  362.         printf("iddle\nref idx:\n");
  363.         for (i = 0; i < NJOINTS; ++i) {
  364.             printf("%u %u ", ref[i], idx[i]);
  365.         }
  366.         putchar('\n');
  367. #endif
  368.         if (calreq) {
  369.             if (!cal(ref, idx)) {
  370.                 zexit(99);
  371.             }
  372.         }
  373.         else {
  374.             for (i = 50000; --i; )
  375.                 ;
  376.             bicio(ARMPWR);
  377.             for (i = 50000; --i; )
  378.                 ;
  379.         }
  380.     }
  381. }
  382.  
  383.  
  384. zexit(n)
  385. {
  386.     register struct drcdevice *dp = DRC_ADDR;
  387.  
  388.     asleep = '1';
  389.     tick = 0;
  390.     dp->drccsr = 0;
  391.     bicio(ARMPWR);
  392. #ifdef DEBUG
  393.     printf("\nexit %d\n", n);
  394. #endif
  395.     halt();
  396. }
  397.