home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL217 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.9 KB  |  68 lines
  1. #include "../h/rccl.h"
  2.  
  3. TRSF_PTR spire;
  4.  
  5. pumatask()
  6. {
  7.     int spirefn();
  8.  
  9.        TRSF_PTR
  10.     base = gentr_rot("BASE", -200., 300., 200., yunit, 0.),
  11.     tool = gentr_rot("TOOL", 0., 0., 180., yunit, 0.),
  12.     washer = gentr_rot("WASHER", 600., 100., 0., zunit, 0.),
  13.     washer_appro = gentr_rot("WASH_APP", 0., 0., 10., yunit, 180.),
  14.     washer_grasp = gentr_rot("WASH_GRASP", 0., 0., 0., yunit, 180.),
  15.     pin = gentr_rot("PIN", 400. , 300., 0., zunit, 0.),
  16.     pinbase = gentr_rot("PINBASE", 0. ,0. , 0., yunit, 180.),
  17.     pintip = gentr_rot("PINTIP", 0., 0., 50., yunit, 180.),
  18.     pin_appro = gentr_rot("PIN_APP", 0., 0., 55., yunit, 180.),
  19.     pin_away = gentr_eul("PIN_AWAY", 0., 10., 20., 20., -20., 0.);
  20.  
  21.     POS_PTR
  22.     approach = makeposition("APPROACH",
  23.         base, t6, tool, EQ, washer, washer_appro, TL, tool),
  24.     getwasher = makeposition("GETWASH",
  25.         base, t6, tool, EQ, washer, washer_grasp, TL, tool),
  26.     above = makeposition("ABOVE",
  27.         base, t6, tool, EQ, pin, pin_appro, TL, tool),
  28.     touch = makeposition("TOUCH",
  29.         base, t6, tool, EQ, pin, pintip, TL, tool),
  30.     down = makeposition("DOWN",
  31.         base, t6, tool, EQ, pin, pinbase, TL, tool),
  32.     away = makeposition("AWAY",
  33.         base, t6, tool, EQ, pin, pinbase, pin_away, TL, tool),
  34.     spiral;
  35.  
  36.  
  37.     spire = newtrans("SPIRE", spirefn);
  38.     spiral = makeposition("SPIRAL",
  39.         base, t6, tool, EQ, spire, pin, pintip, TL, tool);
  40.  
  41.     movejnts(approach, 200, 2000);
  42.     movecart(getwasher, 150, 500);
  43.     movejnts(above, 150, 1000);
  44.     movecart(touch, 150, 500);
  45.     waitfor(completed);
  46.     rtime = 0;
  47.     movecart(spiral, 150, 5500);
  48.     movejnts(down, 200, 1500);
  49.     movecart(away, 200, 1500);
  50.     movejnts(park, 200, 2000);
  51. }
  52.  
  53. /*
  54.  * generate a spiraling motion in the x y plan
  55.  */
  56.  
  57. spirefn()
  58. {
  59.     extern int rtime;
  60.     double time,
  61.            omega = 1.,
  62.            growth = 5.0;
  63.  
  64.     time = (double)rtime / 1000.;
  65.     spire->p.x = time * growth * cos(omega * time * PIT2);
  66.     spire->p.y = time * growth * sin(omega * time * PIT2);
  67. }
  68.