home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 471 / rccl224 < prev    next >
Text File  |  1987-03-02  |  2KB  |  114 lines
  1. #include "../h/rccl.h"
  2. #include "../h/umac.h"
  3. #include "../h/hand.h"
  4.  
  5. #define AWAYZ(p, l)      {distance("dz", - (l)); move(p);}
  6. #define OVERSHOOTZ(p, l) {distance("dz",   (l)); move(p);}
  7. #define FAST             setvel(300, 300.);
  8. #define SLOW             setvel(50, 50.);
  9. #define CAUTIOUS         setvel(7, 7);
  10.  
  11. /*
  12.  * do one insertion
  13.  */
  14.  
  15. insert(z, grip, peg, hole, depth, ang)
  16. TRSF_PTR z, grip, peg, hole;
  17. real depth, ang;
  18. {
  19.     TRSF_PTR bottom, angle, roty;
  20.     POS_PTR align, in, touch;
  21.  
  22.     bottom = gentr_trsl("BOTTOM", 0., 0., -depth);
  23.     angle = gentr_rot("ANGLE", 0., 0., 0., yunit, ang);
  24.     roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
  25.  
  26.     align = makeposition(
  27.     "ALIGN", z, t6, grip, peg, EQ, hole, angle, roty, TL, peg);
  28.  
  29.     touch = makeposition(
  30.     "TOUCH", z, t6, grip, peg, EQ, hole, angle, roty, TL, peg);
  31.  
  32.     in = makeposition(
  33.     "IN", z, t6, grip, peg, EQ, hole, bottom, roty, TL, peg);
  34.  
  35.     setmod('c');
  36.     FAST
  37.     AWAYZ(touch, 10.)
  38.     CAUTIOUS
  39.     AWAYZ(touch, 4.)
  40.     limit("fz", 25.);
  41.     OVERSHOOTZ(touch, 5.)
  42.     comply("fz", 15.);
  43.         move(align);
  44.     lock("fz");
  45.     comply("fx fy", 0., 0.);
  46.         update(hole, in);
  47.         limit("fz", 20.);
  48.         OVERSHOOTZ(in, 10.);
  49.     lock("fx fy");
  50.  
  51.     SLOW
  52.     AWAYZ(align, 50.)
  53.     waitfor(in->end)
  54.     OPEN
  55.     move(there);
  56.     waitfor(completed);
  57.     freepos(align);
  58.     freepos(in);
  59.     freepos(touch);
  60.     freetrans(bottom);
  61.     freetrans(angle);
  62.     freetrans(roty);
  63.     return;
  64. }
  65.  
  66. /*
  67.  * monitors feeder
  68.  */
  69.  
  70. #define PARTS   1
  71. #define EMPTY   2
  72.  
  73. monfeeder()
  74. {
  75.     if (feedersensor == PARTS) {
  76.         nextmove = YES;
  77.         CLOSE
  78.     }
  79.     if (feedersensor == EMPTY) {
  80.         parts = 0;
  81.     }
  82. }
  83.  
  84. /*
  85.  * Do insertions
  86.  */
  87.  
  88. int parts = YES;
  89.  
  90. pumatask()
  91. {
  92.     TRSF_PTR z, e, assy, h, feeder, grasp, pegs;
  93.     POS_PTR get;
  94.  
  95.     z = gentr();                            /* base frame */
  96.     e = gentr();                            /* end effector */
  97.     assy = gentr();                         /* assembly */
  98.     grasp = gentr();                        /* gasp pos */
  99.     feeder = gentr();                       /* feeder */
  100.     pegs = gentr();                         /* peg rel. e */
  101.     h = newtrans("H", hold);                /* h rel. to assy */
  102.  
  103.     get = makeposition("GET", z, t6, e, EQ, feeder, grasp, TL, e);
  104.  
  105.     while(parts) {
  106.         move(get);
  107.         evalfn(monfeeder);
  108.         setime(200, 10000);
  109.         move(get);
  110.         gettr(h, file);
  111.         insert(z, e, pegs, h, 20., 15.);
  112.     }
  113. }
  114.