home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL233 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.4 KB  |  72 lines
  1. #include "../h/rccl.h"
  2. #include "../h/umac.h"
  3. #include "../h/hand.h"
  4.  
  5. int turns;
  6.  
  7. pumatask()
  8. {
  9.     TRSF_PTR z, e, shaft, handle, apro, grasp, rotpx, rotnx;
  10.     POS_PTR  get, away;
  11.     POS_PTR turn;
  12.     int pxfn(), nxfn();
  13.     int q;
  14.  
  15.     rotpx = newtrans("ROTPX", pxfn);
  16.     rotnx = newtrans("ROTNX", nxfn);
  17.     z = gentr_trsl("Z",  0.,  0., 864.);
  18.     e = gentr_trsl("E" , 0. , 0. , 170.);
  19.     shaft = gentr_trsl("SHAFT", -200., 500., 600.);
  20.     shaft->fn = varb;
  21.     handle = gentr_trsl("HANDLE", 0., 0., 50.);
  22.     apro = gentr_trsl("APRO", -50., 0., 0.);
  23.     grasp = gentr_rpy("GRASP", 0., 0., 0., 0., 190., 0.);
  24.  
  25.     get = makeposition(
  26.     "GET", z, t6, e, EQ, shaft, handle, grasp, TL, e);
  27.  
  28.     away = makeposition(
  29.     "AWAY", z, t6, e, EQ, shaft, handle, grasp, apro, TL, e);
  30.  
  31.     turn = makeposition(
  32.     "TURN", z, t6, e, EQ, shaft, rotpx, handle, rotnx, grasp, TL, rotnx);
  33.  
  34.     setvel(300, 300);
  35.     move(away);
  36.     setime(300, 500);
  37.     OPEN;
  38.     if (!teach(shaft, get)) {
  39.         move(away);
  40.         move(park);
  41.         return;
  42.     }
  43.     shaft->fn = const;
  44.     optimize(turn);
  45.     turns = 4;
  46.     waitfor(completed);
  47.     CLOSE;
  48.     comply("fx fz ", 0., 0.);
  49.         movecart(turn, 200, 4000 * turns);
  50.     lock("fx fz ");
  51.     move(get);
  52.     waitfor(turn->end);
  53.     OPEN;
  54.     distance("dx", -30.);
  55.         move(get);
  56.     setvel(200, 50);
  57.     setmod('j');
  58.     move(park);
  59. }
  60.  
  61. pxfn(t)
  62. TRSF_PTR t;
  63. {
  64.     Rot(t, xunit, goalpos->scal * 360 * turns);
  65. }
  66.  
  67. nxfn(t)
  68. TRSF_PTR t;
  69. {
  70.     Rot(t, xunit, - goalpos->scal * 360 * turns);
  71. }
  72.