home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT03.ZIP / RCCL198 < prev    next >
Encoding:
Text File  |  1987-03-02  |  3.2 KB  |  123 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, k1, k2, finf, abov;
  13.     TRSF_PTR b1, b2, b3, b4, b5, over, fing, getit, flip;
  14.     POS_PTR  q1, q2, got;
  15.     POS_PTR  p1, p2, p3, p4, p5, pp1, pp2, pp3, pp4, pp5, get;
  16.     int touchfn();
  17.     int fingfn();
  18.     int q;
  19.  
  20.     finf = newtrans("FINK",fingfn);
  21.     rot(finf, zunit, -90.);
  22.     fing = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
  23.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
  24.     k1 = gentr_rot("K1", 600. ,-300., 450., yunit, 180.);
  25.     k2 = gentr_rot("K2", 600. , 200., 450., yunit, 180.);
  26.     abov = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
  27.     b1 = gentr_trsl("B1", 600. ,-200., 300.);
  28.     b2 = gentr_trsl("B2", 600. ,-100., 300.);
  29.     b3 = gentr_trsl("B3", 600. , 000., 300.);
  30.     b4 = gentr_trsl("B4", 600. , 100., 300.);
  31.     b5 = gentr_trsl("B5", 600. , 200., 300.);
  32.     over = gentr_rot("OVER", 0., 0., 100., yunit, 180.);
  33.     flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);
  34.     getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);
  35.  
  36.  
  37.     q1 = makeposition("Q1" , z, t6, fing, EQ, k1, TL, fing);
  38.     q2 = makeposition("Q2" , z, t6, finf, EQ, k2, TL, finf);
  39.     got = makeposition("GOT", z, t6, EQ, abov, TL, t6);
  40.     p1 = makeposition("P1" , z, t6, fing, EQ, b1, over,TL , fing);
  41.     p2 = makeposition("P2" , z, t6, fing, EQ, b2, over,TL , fing);
  42.     p3 = makeposition("P3" , z, t6, fing, EQ, b3, over,TL , fing);
  43.     p4 = makeposition("P4" , z, t6, fing, EQ, b4, over,TL , fing);
  44.     p5 = makeposition("P5" , z, t6, fing, EQ, b5, over,TL , fing);
  45.     pp1 = makeposition("PP1" , z, t6, fing, EQ, b1, flip, TL , fing);
  46.     pp2 = makeposition("PP2" , z, t6, fing, EQ, b2, flip, TL , fing);
  47.     pp3 = makeposition("PP3" , z, t6, fing, EQ, b3, flip, TL , fing);
  48.     pp4 = makeposition("PP4" , z, t6, fing, EQ, b4, flip, TL , fing);
  49.     pp5 = makeposition("PP5" , z, t6, fing, EQ, b5, flip, TL , fing);
  50.     get = makeposition("GET", z, t6, EQ, getit, TL, t6);
  51.  
  52.  
  53.     movejnts(get, 400, 1500);
  54.     waitfor(completed);
  55.     OPEN;
  56.     printf("put the sensor in the jaws ");
  57.     QUERY(q);
  58.     CLOSE;
  59.     printf("go ahead ");
  60.     QUERY(q);
  61.     if (q == 'n') {
  62.         movejnts(park, 400, 2500);
  63.         return;
  64.     }
  65.     sensor = adcopen(7);
  66.     for (; ; ) {
  67.         movecart(q1, 400, 1500);
  68.         movecart(q2, 400, 10000);
  69.         waitfor(completed);
  70.         QUERY(q); if (q == 'n') break;
  71.         movecart(got, 400, 1500);
  72.     }
  73.     movejnts(get, 400, 1500);
  74.     printf("type 'y' for poking");
  75.     QUERY(q);
  76.     if (q == 'n') {
  77.         movejnts(park, 400, 2500);
  78.         waitfor(completed);
  79.         OPEN;
  80.         return;
  81.     }
  82.     for (; ; ) {
  83.         movejnts(p1, 60, 1500);
  84.         evalfn(touchfn);
  85.         movejnts(pp1, 60, 1500);
  86.         movejnts(p2, 60, 1500);
  87.         evalfn(touchfn);
  88.         movejnts(pp2, 60, 1500);
  89.         movejnts(p3, 60, 1500);
  90.         evalfn(touchfn);
  91.         movejnts(pp3, 60, 1500);
  92.         movejnts(p4, 60, 1500);
  93.         evalfn(touchfn);
  94.         movejnts(pp4, 60, 1500);
  95.         movejnts(p5, 60, 1500);
  96.         evalfn(touchfn);
  97.         movejnts(pp5, 60, 1500);
  98.         movecart(get, 60, 1500);
  99.         printf("more ? ");
  100.         QUERY(q); if (q == 'n') break;
  101.     }
  102.     movejnts(park, 400, 1500);
  103.     waitfor(completed);
  104.     OPEN;
  105. }
  106.  
  107.  
  108. touchfn()
  109. {
  110.     nextmove = how.adcr[sensor] > 1;
  111. }
  112.  
  113.  
  114. fingfn(t)
  115. TRSF_PTR t;
  116. {
  117.     t->p.z += (how.adcr[sensor] * .01 - 3.) / 3.;
  118. }
  119.  
  120.  
  121.  
  122.  
  123.