home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT03.ZIP / RCCL200 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.7 KB  |  79 lines
  1. #include "../h/rccl.h"
  2. #include "../h/rtc.h"
  3. #include "../h/umac.h"
  4.  
  5. extern struct how how;
  6. int sensor;
  7.  
  8. real Y = 0.;
  9.  
  10. pumatask()
  11. {
  12.     TRSF_PTR z, e, circ, tilt, or, fl, b1, conv1;
  13.     POS_PTR  pm3, pm4, pf, p0, p1;
  14.     int conv1fn();
  15.     int circfn(), tiltfn();
  16.     int q;
  17.  
  18.     circ = newtrans("CONV", circfn);
  19.     tilt = newtrans("TILT", tiltfn);
  20.     conv1 = newtrans("CONV1",conv1fn);
  21.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
  22.     e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);
  23.     or = gentr_eul("OR", 300. ,  600., 600.,    0., 0., 90.);
  24.     fl = gentr_rot("FL", 0. , 0., 0., yunit, 180.);
  25.     b1 = gentr_rot("B1", 600. , -300., 500., yunit, 180.);
  26.  
  27.     pm3 = makeposition("PM1" , z, t6, e, EQ, tilt, or, fl, TL, e);
  28.     pm4 = makeposition("PM2" , z, t6, e, EQ, circ, or, fl, TL, e);
  29.     pf = makeposition("PF" , z, t6, e, EQ, or, fl, TL, e);
  30.     p0 = makeposition("P0" , z, t6, e, EQ, b1, TL, e);
  31.     p1 = makeposition("P1", z, t6, e, EQ, conv1, b1, TL, e);
  32.  
  33.     sensor = adcopen(7);
  34.     for (; ; ) {
  35.         movecart(p0, 300, 1500);
  36.         movejnts(p1, 300, 3000);
  37.         movecart(pf, 300, 1500);
  38.         movecart(pm4, 300, 3000);
  39.         movecart(pm3, 300, 3000);
  40.         movecart(park, 300, 1500);
  41.         ++completed;
  42.         while (completed) {
  43.             nap(10);
  44.             nextmove = how.adcr[sensor] > 5;
  45.         }
  46.         Y = 0.;
  47.         QUERY(q); if (q == 'n') break;
  48.     }
  49. }
  50.  
  51.  
  52.  
  53. circfn(t)
  54. TRSF_PTR t;
  55. {
  56.     double radius = 50.;
  57.     t->p.y = radius * sin(2. * goalpos->scal * PIT2);
  58.     t->p.z = radius * cos(2. * goalpos->scal * PIT2);
  59. }
  60.  
  61.  
  62. tiltfn(t)
  63. TRSF_PTR t;
  64. {
  65.     VECT k;
  66.  
  67.     k.x = sin(2. * goalpos->scal * PIT2);
  68.     k.y = cos(2. * goalpos->scal * PIT2);
  69.     k.z = 0.;
  70.     Rot(t, &k, 5.);
  71. }
  72.  
  73.  
  74. conv1fn(t)
  75. TRSF_PTR t;
  76. {
  77.     t->p.y = (Y += 6.);
  78. }
  79.