home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL240 < prev    next >
Encoding:
Text File  |  1987-03-02  |  2.0 KB  |  113 lines
  1. #include "../h/rccl.h"
  2. #include "../h/umac.h"
  3. #include "../h/hand.h"
  4.  
  5. pumatask()
  6. {
  7.     TRSF_PTR z, e, peg, hole, roty, bottom, angle;
  8.     POS_PTR align, in, touch;
  9.     int q;
  10.  
  11.     z = gentr_trsl("Z",  0.,  0., 864.);
  12.     e = gentr_trsl("E" , 0. , 0. , 140.);
  13.     peg = gentr_trsl("PEG", 0., 0., 10.);
  14.     hole = gentr_trsl("HOLE", -50., 450., 500.);
  15.     hole->fn = varb;
  16.     bottom = gentr_trsl("OVER", 0., 0., -20.);
  17.     roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
  18.     angle = gentr_rot("ANGLE", 0., 0., 0., yunit, 12.);
  19.  
  20.     align = makeposition(
  21.     "ALIGN", z, t6, e, peg, EQ, hole, roty, TL, peg);
  22.  
  23.     touch = makeposition(
  24.     "TOUCH", z, t6, e, peg, EQ, hole, angle, roty, TL, peg);
  25.  
  26.     in = makeposition(
  27.     "IN", z, t6, e, peg, EQ, hole, bottom, roty, TL, peg);
  28.  
  29.     setvel(300, 50);
  30.     move(align);
  31.     if (!teach(hole, align)) {
  32.         setvel(300, 50);
  33.  
  34.         distance("dz", -100.);
  35.             move(align);
  36.  
  37.         setmod('j');
  38.         move(park);
  39.         return;
  40.     }
  41.     setmod('c');
  42.     setvel(100, 100);
  43.  
  44.     distance("dz", -10.);
  45.         move(touch);
  46.  
  47.     QUERY(q);
  48.  
  49.     if (q == 'n') {
  50.         setvel(300, 100);
  51.         setmod('j');
  52.         move(park);
  53.         return;
  54.     }
  55.  
  56.  
  57.     setvel(4, 7);
  58.     distance("dz", -4.);
  59.         move(touch);
  60.  
  61.     limit("fz", 25.);
  62.     distance("dz", 5.);
  63.         move(touch);
  64.  
  65.     comply("fz", 15.);
  66.         move(align);
  67.     lock("fz");
  68.  
  69.     comply("fx fy", 0., 0.);
  70.         distance("dz", 5.);
  71.             move(align);
  72.  
  73.         limit("fz", 20.);
  74.         move(in);
  75.  
  76.         update(hole, in);
  77.         limit("fz tx", 40., 10.);
  78.         distance("rx", 2.);
  79.             move(in);
  80.  
  81.         update(hole, in);
  82.         limit("fz tx", 40., 10.);
  83.         distance("rx", -2.);
  84.             move(in);
  85.  
  86.         update(hole, in);
  87.         limit("fz ty", 40., 10.);
  88.         distance("ry", 2.);
  89.             move(in);
  90.  
  91.         update(hole, in);
  92.         limit("fz ty", 40., 10.);
  93.         distance("ry", -2.);
  94.             move(in);
  95.  
  96.         update(hole, in);
  97.         limit("fz", 20.);
  98.         distance("dz", 10.);
  99.             move(in);
  100.  
  101.         limit("dx dy", 1., 1.);
  102.         move(align);
  103.     lock("fx fy");
  104.  
  105.     setvel(50, 50);
  106.     distance("dz", -50.);
  107.         move(align);
  108.  
  109.     setvel(300, 50);
  110.     setmod('j');
  111.     move(park);
  112. }
  113.