home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL231 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.8 KB  |  93 lines
  1. #include "../h/rccl.h"
  2. #include "../h/hand.h"
  3. #include "../h/rtc.h"
  4. #include "../h/umac.h"
  5.  
  6. extern struct how how;
  7.  
  8. static int sensor;
  9.  
  10. #define TOUCHED 10
  11.  
  12. pumatask()
  13. {
  14.     int touchfn();
  15.     TRSF_PTR z, b1, b2, fing, getit, flip;
  16.     POS_PTR  get, p1, p2;
  17.     int q;
  18.  
  19.     z = gentr_trsl("Z",  0.,  0., 864.);
  20.     b1 = gentr_trsl("B1", 600. ,-200., 400.);
  21.     b2 = gentr_trsl("B2", 600. ,-100., 400.);
  22.     fing = gentr_rot("FING", 0., 0., 200., zunit, -90.);
  23.     getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);
  24.     flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);
  25.  
  26.     p1 = makeposition("P1" , z, t6, fing, EQ, b1, flip,TL , fing);
  27.     p2 = makeposition("P2" , z, t6, fing, EQ, b2, flip,TL , fing);
  28.     get = makeposition("GET", z, t6, EQ, getit, TL, t6);
  29.  
  30.     setvel(300, 100);
  31.     move(get);
  32.     waitfor(completed);
  33.     OPEN;
  34.     printf("put the sensor in the jaws ");
  35.     QUERY(q);
  36.     CLOSE;
  37.     printf("go ahead ");
  38.     QUERY(q);
  39.     if (q == 'n') {
  40.         move(park);
  41.         return;
  42.     }
  43.     sensor = adcopen(7);
  44.     setvel(100, 100);
  45.     for (; ; ) {
  46.         p1->end = p2->end = 0;
  47.         move(p1);
  48.         evalfn(touchfn);
  49.         setime(0, 0);
  50.         distance("dz", 100.);
  51.         move(p1);
  52.         move(p1);
  53.  
  54.         move(p2);
  55.         evalfn(touchfn);
  56.         setime(0, 0);
  57.         distance("dz", 100.);
  58.         move(p2);
  59.         move(p2);
  60.  
  61.         waitfor(p1->end)
  62.         printf("guarded motion 1 starts\n");
  63.         waitfor(p1->end)
  64.         if (p1->code == TOUCHED)
  65.             printf("touched\n");
  66.         else
  67.             printf("not touched\n");
  68.  
  69.         waitfor(p2->end)
  70.         printf("guarded motion 2 starts\n");
  71.         waitfor(p2->end)
  72.         if (p2->code == TOUCHED)
  73.             printf("touched\n");
  74.         else
  75.             printf("not touched\n");
  76.  
  77.         printf("more ? ");
  78.         QUERY(q); if (q == 'n') break;
  79.     }
  80.     setvel(300, 100);
  81.     move(park);
  82.     waitfor(completed);
  83.     OPEN;
  84. }
  85.  
  86.  
  87. touchfn()
  88. {
  89.     if (how.adcr[sensor] > 1) {
  90.         nextmove = TOUCHED;
  91.     }
  92. }
  93.