home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL219 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.3 KB  |  68 lines
  1. #include "../h/rccl.h"
  2. #include "../h/rtc.h"
  3. #include "../h/hand.h"
  4. #include "../h/umac.h"
  5.  
  6. extern struct how how;
  7. extern struct chg chg;
  8. int sensor;
  9.  
  10. pumatask()
  11. {
  12.     TRSF_PTR z, b1, b2,fing, fins, over;
  13.     POS_PTR  p1, p2, get;
  14.     int fingfn();
  15.     int q;
  16.  
  17.     fing = newtrans("FING",fingfn);
  18.     Rot(fing, zunit, -90.);
  19.     fins = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
  20.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
  21.     b1 = gentr_rot("B1", 600. ,-300., 450., yunit, 180.);
  22.     b2 = gentr_rot("B2", 600. , 300., 450., yunit, 180.);
  23.     over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
  24.  
  25.     b1->fn = hold;
  26.     p1 = makeposition("P1" , z, t6, fins, EQ, b1, TL, fins);
  27.     p2 = makeposition("P1" , z, t6, fing, EQ, b2, TL, fing);
  28.     get = makeposition("GET", z, t6, EQ, over, TL, t6);
  29.  
  30.  
  31.     setvel(400, 300);
  32.     move(get);
  33.     waitfor(completed);
  34.     OPEN
  35.     printf("put the sensor ");
  36.     QUERY(q);
  37.     CLOSE
  38.     printf("go ahead ");
  39.     QUERY(q);
  40.     if (q == 'n') {
  41.         move(park);
  42.         return;
  43.     }
  44.     sensor = adcopen(7);
  45.     for (; ; ) {
  46.         setmod('c');
  47.         move(p1);
  48.         stop(0);
  49.         sample(15);
  50.         movecart(p2, 200, 8000);
  51.         sample(30);
  52.         waitfor(completed);
  53.         QUERY(q); if (q == 'n') break;
  54.         move(get);
  55.     }
  56.     OPEN
  57.     setmod('j');
  58.     move(park);
  59. }
  60.  
  61. #define CV      .010
  62.  
  63. fingfn(t)
  64. TRSF_PTR t;
  65. {
  66.     t->p.z += (how.adcr[sensor] * CV - 3.) / 3.;
  67. }
  68.