home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT03.ZIP / RCCL199 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.8 KB  |  90 lines
  1. #include "../h/rccl.h"
  2. #include "../h/rtc.h"
  3. #include "../h/hand.h"
  4. #include "../h/umac.h"
  5.  
  6. extern struct how how;
  7. extern struct chg chg;
  8. int sensor;
  9.  
  10. pumatask()
  11. {
  12.     TRSF_PTR z, b1, e, h, search, track, pt, over;
  13.     POS_PTR  p1, p2, p3, get;
  14.     int searchfn(), trackfn();
  15.     int q;
  16.  
  17.     search = newtrans("FING",searchfn);
  18.     track = newtrans("FING",trackfn);
  19.     h = newtrans("H", const);
  20.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
  21.     e = gentr_eul("E" , 0. , 0. , 200. , 0. , 0.,  0.);
  22.     b1 = gentr_rot("B1", 600. ,-400., 450., yunit, 180.);
  23.     pt = gentr_rot("PT", 0., 0., 0., zunit, 90.);
  24.     over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
  25.  
  26.     p1 = makeposition("P1" , z, t6, e, EQ, b1, pt, TL, pt);
  27.     p2 = makeposition("P2" , z, t6, e, search, EQ, b1, pt, TL, pt);
  28.     p3 = makeposition("P3" , z, t6, e, h, track, EQ, b1, pt, TL, e);
  29.     get = makeposition("GET", z, t6, EQ, over, TL, t6);
  30.  
  31.  
  32.     movejnts(get, 400, 1500);
  33.     waitfor(completed);
  34.     OPEN
  35.     printf("put the sensor ");
  36.     QUERY(q);
  37.     CLOSE
  38.     printf("go ahead ");
  39.     QUERY(q);
  40.     if (q == 'n') {
  41.         movejnts(park, 100, 20000);
  42.         return;
  43.     }
  44.     sensor = adcopen(7);
  45.     movecart(p1, 400, 1500);
  46.     movecart(p2, 100, 20000);
  47.     movecart(p3, 100, 20000);
  48.     OPEN
  49.     movejnts(park, 400, 1500);
  50. }
  51.  
  52. #define CV      .010
  53. #define STEP   1.
  54.  
  55. double ZT;
  56.  
  57. searchfn(t)
  58. TRSF_PTR t;
  59. {
  60.     double alpha = 0., inc;
  61.     int fl;
  62.  
  63.     t->p.z -= STEP;
  64.     fl = how.adcr[sensor];
  65.     if ((fl * CV - 2.5) * .1 > 0.) {
  66.         nextmove = YES;
  67.         ZT = t->p.z;
  68.     }
  69. }
  70.  
  71. trackfn(t)
  72. TRSF_PTR t;
  73. {
  74.     double alpha = 0., inc;
  75.     int fl;
  76.     static int first = YES;
  77.  
  78.     if (first) {
  79.         first = NO;
  80.         t->p.z = ZT;
  81.     }
  82.     t->p.x -= STEP;
  83.     fl = how.adcr[sensor];
  84.     inc = (fl * CV - 2.5);
  85.     t->p.z += inc * .05;
  86.     alpha = atan2(inc , STEP);
  87.     alpha *= RADTODEG;
  88.     rotm(t, yunit, -alpha);
  89. }
  90.