home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL064 < prev    next >
Encoding:
Text File  |  1987-03-02  |  2.7 KB  |  168 lines
  1. /*
  2.  * MAC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : mac
  6.  *      File    : mcal.c
  7.  *      Remarks : calibration function
  8.  *      Usage   : part of maint and moper
  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. #define YES     1
  22. #define NO      0
  23.  
  24. extern short (* onclk)();
  25. extern short tick;
  26.  
  27. unsigned short index[NJOINTS];
  28. unsigned short calpos[NJOINTS];
  29.  
  30. static short timeout;
  31.  
  32. static short calstep;
  33.  
  34.  
  35. static unsigned short calrefp[NJOINTS], calidxp[NJOINTS], off[NJOINTS];
  36.  
  37. cal(refp, idxp)
  38. unsigned short *refp, *idxp;
  39. {
  40.     short armcal(), steper(), refarm();
  41.     register short i;
  42.  
  43.     tick = 0;
  44.     for (i = 0; i < NJOINTS; ++i) {
  45.         calrefp[i] = refp[i];
  46.         calidxp[i] = idxp[i];
  47.         off[i] = YES;
  48.     }
  49.     put6j(STOPM, calrefp);
  50.     get6j(READ, calpos);
  51.  
  52.     if (!getio(ARMPWR)) {
  53.         bisio(ARMPWR);
  54.         while(!getio(ARMPWR))
  55.                ;
  56.     }
  57.  
  58.     put6pg(MODE, MESERVO|MEPOSSV);
  59.     get6j(READ, calpos);
  60.  
  61.     onclk = steper;
  62.     timeout = 7;
  63.     calstep = -16;
  64.     tick = TACK;
  65.     while(tick) {
  66.         wait();
  67.     }
  68.     put6pg(MODE, MESERVO|MEPOSSV|MLOOKZI);
  69.     onclk = armcal;
  70.     timeout = 250;
  71.     calstep = 16;
  72.     tick = TACK;
  73.     while(tick) {
  74.         wait();
  75.     }
  76.     if(timeout) {
  77.         put6j(STOPM, calidxp);
  78.         for (i = 0; i < NJOINTS; ++i) {
  79.             calpos[i] = calidxp[i];
  80.         }
  81.         onclk = refarm;
  82.         calstep = 16;
  83.         tick = TACK;
  84.         while (tick) {
  85.             wait();
  86.         }
  87.         put6j(POSET, calrefp);
  88.         return(YES);
  89.     }
  90.     else {
  91.         return(NO);
  92.     }
  93. }
  94.  
  95.  
  96. short armcal()
  97. {
  98.     register short i = NJOINTS;
  99.     unsigned short stat[NJOINTS];
  100.  
  101.     if (--timeout) {
  102.         get6j(JSTAT, stat);
  103.         while (i > 0 && stat[--i] & JFNDIXS)
  104.                ;
  105.         if (i) {
  106.             for(i = 0; i < NJOINTS; ++i)
  107.                 calpos[i] += calstep;
  108.             put6j(POSET, calpos);
  109.         }
  110.         else {
  111.             tick = 0;
  112.         }
  113.     }
  114.     else {
  115.         tick = 0;
  116.     }
  117. }
  118.  
  119.  
  120. short steper()
  121. {
  122.     register short i;
  123.  
  124.     if (--timeout) {
  125.         for(i = 0; i < NJOINTS; ++i)
  126.             calpos[i] += calstep;
  127.         put6j(POSET, calpos);
  128.     }
  129.     else {
  130.         tick = 0;
  131.     }
  132. }
  133.  
  134.  
  135.  
  136. short refarm()
  137. {
  138.     short i, done = YES;
  139.  
  140.     for (i = 0; i < NJOINTS; ++i) {
  141.         if (off[i]) {
  142.             done = NO;
  143.         }
  144.         else {
  145.             continue;
  146.         }
  147.         if (calidxp[i] > calrefp[i]) {
  148.             calpos[i] -= calstep;
  149.             if (calpos[i] < calrefp[i]) {
  150.                 off[i] = NO;
  151.             }
  152.         }
  153.         if (calidxp[i] < calrefp[i]) {
  154.             calpos[i] += calstep;
  155.             if (calpos[i] > calrefp[i]) {
  156.                 off[i] = NO;
  157.             }
  158.         }
  159.         if (calidxp[i] == calrefp[i]) {
  160.             off[i] = NO;
  161.         }
  162.     }
  163.     put6j(POSET, calpos);
  164.     if (done) {
  165.         tick = 0;
  166.     }
  167. }
  168.