home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL065 < prev    next >
Encoding:
Text File  |  1987-03-02  |  6.9 KB  |  364 lines
  1. /*
  2.  * MAC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : mac
  6.  *      File    : maint.c
  7.  *      Remarks : maintenance program
  8.  *      Usage   : make maint
  9.  */
  10.  
  11. #include "../h/which.h"
  12. #include "../h/addefs.h"
  13. #include "../h/jcom.h"
  14.  
  15. #ifdef VALII
  16. #define TACK    8
  17. #else
  18. #define TACK    4
  19. #endif
  20.  
  21. /*            global variables shared with the assembly code                */
  22.  
  23. extern short (* onclk)();
  24. extern short tick;
  25.  
  26.  
  27. /****************************************************************************/
  28. /*                                                                          */
  29. /*              manual mode                                                 */
  30. /*                                                                          */
  31. /****************************************************************************/
  32.  
  33. #define MAXMAG 80
  34. #define HANDSTEP 10
  35. #define YES 1
  36. #define NO 0
  37.  
  38. #define HOME  '\036'
  39. #define UP    '\013'
  40. #define DOWN  '\012'
  41. #define LEFT  '\014'
  42. #define BS    '\010'
  43. #define CLEAR '\032'
  44.  
  45. static short curc[6] = { NO, NO, NO, NO, NO, NO };
  46. static unsigned short poshd = 0;
  47. static unsigned short possetp[6];
  48. static unsigned short readpos[6];
  49. static unsigned short trqsetp[6] = {0, 0, 0, 0, 0, 0};
  50. static short dcount[6];
  51. static short mag = 0;
  52. static short nopower;
  53.  
  54. maint()
  55. {
  56.     short j, c;
  57.  
  58.     tick = 0;
  59.     for (j = 0; j < 6; ++j)
  60.         curc[j] = NO;
  61.     do {
  62.         printf("\ndo you want to free a joint (y/n) ");
  63.         c = getchar();
  64.         putchar('\n');
  65.         if (c == 'y') {
  66.             printf("joint number [1-6] :");
  67.             if ((j = getchar()) <= '6' && j >= '1') {
  68.                 curc[j -= '1'] = YES;
  69.                 printf("\njoint %d freed\n", j + 1);
  70.             }
  71.         }
  72.     } while(c == 'y');
  73.     manu();
  74. }
  75.  
  76.  
  77. manu()
  78. {
  79.     short ptfn();
  80.  
  81.     short c, j, sig = 1, more = 1, value;
  82.  
  83.     tick = 0;
  84.  
  85.     printf("\nPRESS RETURN TO ENTER MANUAL MODE ");
  86.     while(getchar() != '\n')
  87.         ;
  88. restart:
  89.     tick = 0;
  90.     putchar(CLEAR);
  91.  
  92.     put6j(STOPP, possetp);
  93.     put6pg(MODE, MESERVO);
  94.     get6j(READ, readpos);
  95. #ifdef STAN
  96.     putp(SCALE, 3, 2);
  97.  
  98.     putp(IGAIN, 0, 3);      /* due to enormous backlash J4 */
  99.     putp(VGAIN, 0, 3);
  100.     putp(SCALE, 2, 3);
  101.  
  102.     putp(MODE, MESERVO, 6);
  103. #endif
  104.     if (!getio(ARMPWR)) {
  105.         printf("\nTurn on Arm Power\n");
  106.         bisio(ARMPWR);
  107.         while(!getio(ARMPWR)) {
  108.             get6j(READ, readpos);
  109.             wait();
  110.         }
  111.     }
  112.     for (j = 0; j < 6; ++j) {
  113.         dcount[j] = 0;
  114.         possetp[j] = readpos[j];
  115.     }
  116.     poshd = 0;
  117.     mag = 0;
  118.     onclk = ptfn;
  119.     tick = TACK;
  120.  
  121.     putchar(CLEAR);
  122.     printf(
  123. "\n\n\n\n\n\n\n\n\n\n\n   FASTER SLOWER REV JOINTS FREE LOCK HAND  EXIT\n");
  124.     printf(
  125.               "    '.'    ','   '-'  [1-6] 'f'  'l'  o/c return\n");
  126.  
  127.     putchar(HOME);
  128.     printf("\n\n\n");
  129.     for ( c = 0; c < 6 ; ++c)
  130.         printf(
  131.       "%1d %c %6u\n",c + 1, (curc[c]) ? 'F' : 'L', readpos[c]);
  132.  
  133.     while (more) {
  134.         if (nopower)
  135.             goto restart;
  136.         putchar(HOME);
  137.         c = getchar();
  138.         putchar(LEFT);
  139.         switch (c) {
  140.         case '1' :
  141.         case '2' :
  142.         case '3' :
  143.         case '4' :
  144.         case '5' :
  145.         case '6' :
  146.             if (!curc[c -= '1']) {
  147.                 dcount[c] += (sig) ? mag : -mag;
  148.             }
  149.             printf("\n\n\n");
  150.             for (j = c; j--;)
  151.                 putchar('\n');
  152.             printf(
  153.       "%1d %c %6u",c + 1, (curc[c]) ? 'F' : 'L', readpos[c]);
  154.             break;
  155. #ifdef PUMA
  156.         case 'o' :
  157.             poshd = 'o';
  158.             break;
  159.         case 'c' :
  160.             poshd = 'c';
  161.             break;
  162. #endif
  163. #ifdef STAN
  164.         case 'o' :
  165.             poshd += HANDSTEP;
  166.             break;
  167.         case 'c' :
  168.             poshd -= HANDSTEP;
  169.             break;
  170. #endif
  171.         case '.' :
  172.             (mag < MAXMAG) ? ++mag : mag;
  173.             break;
  174.         case ',' :
  175.             (mag) ? --mag : mag;
  176.             break;
  177.         case '-' :
  178.             sig = !sig;
  179.             break;
  180.         case '\n' :
  181.             printf("%c   exit ? (y/n) ", HOME);
  182.             if (getchar() == 'y')
  183.                 more = 0;
  184.             else
  185.                 printf("%c                  ", HOME);
  186.             break;
  187.         case 'f' :
  188.             printf("%c    free joint [1-6] :", HOME);
  189.             if ((c = getchar()) <= '6' && c >= '0') {
  190.                 printf("\n\n\n");
  191.                 if (!curc[c -= '1'])
  192.                     curc[c] = YES;
  193.                 for (j = c; j--;)
  194.                     putchar('\n');
  195.                 printf("%1d F",c + 1);
  196.             }
  197.             printf("%c                        ", HOME);
  198.             break;
  199.         case 'l' :
  200.             printf("%c    lock joint [1-6] :", HOME);
  201.             if ((c = getchar()) <= '6' && c >= '0') {
  202.                 printf("\n\n\n");
  203.                 if (curc[c -= '1'])
  204.                     curc[c] = NO;
  205.                 for (j = c; j--;)
  206.                     putchar('\n');
  207.                 printf("%1d L",c + 1);
  208.             }
  209.             printf("%c                        ", HOME);
  210.         default :
  211.             break;
  212.         }
  213.     }
  214.     tick = 0;
  215.     putchar(CLEAR);
  216. }
  217.  
  218.  
  219.  
  220.  
  221.  
  222. short ptfn()
  223. {
  224.     register short j;
  225.  
  226.     get6j(READ, readpos);
  227.     if (nopower = !getio(ARMPWR)) {
  228.         return;
  229.     }
  230.     for (j = 0; j < 6; ++j) {
  231.         if (curc[j]) {
  232.             putj(DACSET, trqsetp[j], j);
  233.             getj(READ, possetp[j], j);
  234.         }
  235.         else {
  236.             if (dcount[j] > 0) {
  237.                 if (possetp[j] < MAXMAG) {
  238.                     possetp[j] = 077777;
  239.                     putj(STOPM, 077777, j);
  240.                 }
  241.                 dcount[j] -= mag;
  242.                 possetp[j] -= mag;
  243.             }
  244.             if (dcount[j] < 0) {
  245.                 if (possetp[j] > 0177777-MAXMAG) {
  246.                     possetp[j] = 077777;
  247.                     putj(STOPM, 077777, j);
  248.                 }
  249.                 dcount[j] += mag;
  250.                 possetp[j] += mag;
  251.             }
  252.             putj(POSET, possetp[j], j);
  253.         }
  254.     }
  255. #ifdef STAN
  256.     putj(DACSET, poshd, 6);
  257. #endif
  258. #ifdef PUMA
  259.     if (poshd == 'o') {
  260.         bicio(HNDOH);
  261.         poshd = NO;
  262.     }
  263.     if (poshd == 'c') {
  264.         bisio(HNDOH);
  265.         poshd = NO;
  266.     }
  267. #endif
  268. }
  269.  
  270.  
  271.  
  272.  
  273.  
  274.  
  275. printb(i, g)
  276. short i, g;
  277. {
  278.     short n = 16;
  279.  
  280.     for (; n--; i<<=1) {
  281.         if (n != 15 && n%g == g-1)
  282.             putchar(' ');
  283.         if (i < 0)
  284.             putchar('1');
  285.         else
  286.             putchar('0');
  287.     }
  288. }
  289.  
  290.  
  291.  
  292.  
  293. #include "../h/stdio11.h"
  294. #include "../h/which.h"
  295. #ifdef PUMA
  296. #include "../h/pumadata.h"
  297. #endif
  298. #ifdef STAN
  299. #include "../h/standata.h"
  300. #endif
  301.  
  302. static unsigned short ref[6] = {ECCL1, ECCL2, ECCL3, ECCL4, ECCL5, ECCL6};
  303. static unsigned short idx[6] = {XCCL1, XCCL2, XCCL3, XCCL4, XCCL5, XCCL6};
  304.  
  305.  
  306. main()
  307. {
  308.     extern unsigned short *calpos, *index;
  309.     short i;
  310.  
  311.     setbuf(stdout, NULL);
  312.     setbuf(stdin, NULL);
  313.     printf("\nflip switch to Micro and type <return> to begin ");
  314.     while (getchar() != '\n')
  315.         ;
  316.     do {
  317.         maint();
  318.         printf("\ndo you want to calibrate (y/n) ");
  319.         if (getchar() == 'y') {
  320.             putchar('\n');
  321.             if (cal(ref, idx)) {
  322.                 printf("\nThe arm is now calibrated\n");
  323.                 get6j(READ, calpos);
  324.                 get6j(JSTAT, index);
  325.                 printf("status  position\n");
  326.                 for (i=0; i < 6; i++) {
  327.                     printb(index[i], 4);
  328.                     printf("  %u\n", calpos[i]);
  329.                 }
  330.             }
  331.             else {
  332.                 get6j(JSTAT, index);
  333.                 for (i=0; i < 6; i++) {
  334.                     if (index[i] & JLOOKZI) {
  335.                         printf("No index joint %d\n", i+1);
  336.                      }
  337.                 }
  338.                 printf("Error - try again\n");
  339.                 for (i=0; i<6; i++) {
  340.                     printb(index[i], 4);
  341.                     putchar('\n');
  342.                 }
  343.                 putchar('\n');
  344.             }
  345.         }
  346.         printf("\nmore (y/n) ");
  347.     } while (getchar() == 'y');
  348. }
  349.  
  350.  
  351.  
  352. #define CLKAD           ((unsigned short *)0100)
  353. #define CLKPS           ((unsigned short *)0102)
  354.  
  355. zexit(n)
  356. {
  357.     tick = 0;
  358.     *CLKPS = 2;
  359.     *CLKAD = CLKPS;
  360.     bicio(ARMPWR);
  361.     printf("\nexit %d\n", n);
  362.     halt();
  363. }
  364.