home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL246 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.4 KB  |  72 lines
  1. #include "../h/rccl.h"
  2. #include "../h/hand.h"
  3. #include "../h/which.h"
  4. #include "../h/kine.h"
  5.  
  6. pumatask()
  7. {
  8.     TRSF_PTR  z, e, cam, o, coord, t6r, u5i, expect, drop;
  9.     POS_PTR look, get, put;
  10.  
  11.     z = gentr_trsl("Z",  0.,  0., 864.);
  12.     e = gentr_trsl("E" , 0. , 0. , 170.);
  13.     cam = gentr_rot("CAM", 0., 0., 50., xunit, 90.);
  14.     expect = gentr_rot("EXPECT", 500. , 100., 600., yunit, 180.);
  15.     drop = gentr_rot("DROP", 400. , -100., 500., yunit, 180.);
  16.     o = newtrans("O", hold);
  17.     coord = newtrans("COORD", hold);
  18.  
  19.     u5i = newtrans("U5I", varb);
  20.     t6r = newtrans("T6R", varb);
  21.  
  22.     look = makeposition("LOOK", z, t6, e, EQ, expect, TL, e);
  23.     get = makeposition("GET", t6, e, EQ, coord, cam, o, TL, e);
  24.     put = makeposition("PUT" , z, t6, e, EQ, drop, TL, e);
  25.  
  26.  
  27.     setvel(200, 100);
  28.     for (; ; ) {
  29.         move(look);
  30.         waitas(goalpos == look && look->scal > .8);
  31.         snapshot();
  32.         Assigntr(t6r, t6);
  33.         Invert(u5i, &sncs_d.u5);
  34.         Trmult(coord, t6r, u5i);
  35.         if (!getobj(o)) {
  36.             break;
  37.         }
  38.         get->end = 0;
  39.         distance("dz", -30.);
  40.             move(get);
  41.         move(get);
  42.         stop(50);
  43.         distance("dz", -30.);
  44.             move(get);
  45.  
  46.         waitfor(get->end);
  47.         waitfor(get->end);
  48.         CLOSE;
  49.         printf("closing\n");
  50.         move(put);
  51.         waitfor(put->end);
  52.         OPEN;
  53.         printf("openning\n");
  54.     }
  55.     move(park);
  56. }
  57.  
  58. snapshot()
  59. {
  60.     printf("snap\n");
  61. }
  62.  
  63. getobj(t)
  64. TRSF_PTR t;
  65. {
  66.     static int number = 5;
  67.  
  68.     Trsl(t, 0., 0., 200. + number * 30.);
  69.     Rot(t, yunit, 10. * number);
  70.     return(number--);
  71. }
  72.