home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL223 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.3 KB  |  84 lines
  1. #include "../h/rccl.h"
  2. #include "../h/hand.h"
  3. #include "../h/umac.h"
  4.  
  5. real when;
  6.  
  7. #define MAXACC  .015    /* mm/ms2 */
  8.  
  9. throw(v0)
  10. VECT_PTR v0;
  11. {
  12.     int openat();
  13.  
  14.     real Tx = (12. * v0->x) / MAXACC;
  15.     real Ty = (12. * v0->y) / MAXACC;
  16.     real Tz = (12. * v0->z) / MAXACC;
  17.     int  T = ((FABS(Tx) > (Ty))
  18.             ? ((FABS(Tx) > FABS(Tz))
  19.                 ? Tx : Tz)
  20.             : ((FABS(Ty) > FABS(Tz))
  21.                 ? Ty : Tz));
  22.  
  23.     real dx, dy, dz;
  24.  
  25.     stop(0);
  26.     setmod('c');
  27.     dx = Tx * v0->x / 2.;
  28.     dy = Ty * v0->y / 2.;
  29.     dz = Tz * v0->z / 2.;
  30.  
  31.     distance("dx dy dz", -dx, -dy, -dz);
  32.     setime(T / 2, T);
  33.     move(there);
  34.  
  35.     when = .90;
  36.     evalfn(openat);
  37.     distance("dx dy dz",  2. * dx,  2. * dy,  2. * dz);
  38.     setime(T / 2, T);
  39.     move(there);
  40.  
  41.     setime(T / 2, T);
  42.     move(there);
  43.     stop(0);
  44.     return;
  45. }
  46.  
  47.  
  48. openat()
  49. {
  50.     if (goalpos->scal >= when) {
  51.         OPEN;
  52.     }
  53. }
  54.  
  55.  
  56. pumatask()
  57. {
  58.     TRSF_PTR b0, grip;
  59.     POS_PTR p0;
  60.     VECT vel;
  61.     int q;
  62.  
  63.     grip = gentr_trsl("GRIP", 0., 0., 170.);
  64.     b0 = gentr_rot("B0", 400., 150., 700., yunit, 45.);
  65.  
  66.     p0 = makeposition("P0", t6, grip, EQ, b0, TL ,grip);
  67.  
  68.     QUERY(q)
  69.     CLOSE
  70.     setconf("d");
  71.     setime(100, 3000);
  72.     move(p0);
  73.     vel.x = .0;
  74.     vel.y = .0;
  75.     vel.z = .6;
  76.  
  77.     throw(&vel);
  78.  
  79.     setmod('j');
  80.     setconf("u");
  81.     setime(100, 3000);
  82.     move(park);
  83. }
  84.