home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT04.ZIP / RCCL248 < prev    next >
Encoding:
Text File  |  1987-03-02  |  3.3 KB  |  167 lines
  1. #include "../h/rccl.h"
  2. #include "../h/rtc.h"
  3. #include "../h/which.h"
  4. #include "../h/kine.h"
  5. #include "../h/umac.h"
  6.  
  7. extern struct how how;
  8. extern struct chg chg;
  9.  
  10. int idx = 0;
  11. char outc;
  12.  
  13. #define MAX     1000
  14.  
  15. double cur1[MAX];
  16. double cur2[MAX];
  17. double cur3[MAX];
  18. double cur4[MAX];
  19. double cur5[MAX];
  20. double cur6[MAX];
  21. double tor1[MAX];
  22. double tor2[MAX];
  23. double tor3[MAX];
  24. double tor4[MAX];
  25. double tor5[MAX];
  26. double tor6[MAX];
  27. char   cccc[MAX];
  28.  
  29. pumatask()
  30. {
  31.     TRSF_PTR z, e, beg, end;
  32.     POS_PTR  p1, p2;
  33.     int i;
  34.     int fc1, fc2, fc3, fc4, fc5, fc6;
  35.     int ft1, ft2, ft3, ft4, ft5, ft6;
  36.     int fcc;
  37.     int record();
  38.  
  39.     e = trsl(newtrans("E", const), 0., 0., 170.);
  40.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
  41.     beg = newtrans("BEG", varb);
  42.     end = newtrans("END", varb);
  43.  
  44.  
  45.     p1 = makeposition("P2", z, t6, e, EQ ,beg, TL, e);
  46.     p2 = makeposition("P3", z, t6, e, EQ, end, TL, e);
  47.  
  48.  
  49.  
  50.     setvel(700, 700);
  51.     setmod('j');
  52.     if (!teach(beg,  p1)) {
  53.         setvel(700, 700);
  54.         setmod('j');
  55.         move(park);
  56.         return;
  57.     }
  58.     if (!teach(end,  p2)) {
  59.         setvel(700, 700);
  60.         setmod('j');
  61.         move(park);
  62.         return;
  63.     }
  64.     setmod('c');
  65.     move(p2);
  66.  
  67.     setvel(70, 20);
  68.  
  69.     outc = '-';
  70.     evalfn(record);
  71.     move(p1);
  72.     waitfor(completed);
  73.  
  74.     outc = '+';
  75.     evalfn(record);
  76.     move(p2);
  77.     waitfor(completed);
  78.  
  79.     setvel(700, 700);
  80.     move(park);
  81.     stop(100);
  82.     waitfor(completed);
  83.     release("happy");
  84.     fcc = creat("../g/c.out", 0644);
  85.     write(fcc, cccc, idx);
  86.     close(fcc);
  87.     ft1 = creat("../g/tor1.out", 0644);
  88.     ft2 = creat("../g/tor2.out", 0644);
  89.     ft3 = creat("../g/tor3.out", 0644);
  90.     ft4 = creat("../g/tor4.out", 0644);
  91.     ft5 = creat("../g/tor5.out", 0644);
  92.     ft6 = creat("../g/tor6.out", 0644);
  93.     write(ft1, tor1, idx * sizeof(double));
  94.     write(ft2, tor2, idx * sizeof(double));
  95.     write(ft3, tor3, idx * sizeof(double));
  96.     write(ft4, tor4, idx * sizeof(double));
  97.     write(ft5, tor5, idx * sizeof(double));
  98.     write(ft6, tor6, idx * sizeof(double));
  99.     close(ft1);
  100.     close(ft2);
  101.     close(ft3);
  102.     close(ft4);
  103.     close(ft5);
  104.     close(ft6);
  105.     fc1 = creat("../g/cur1.out", 0644);
  106.     fc2 = creat("../g/cur2.out", 0644);
  107.     fc3 = creat("../g/cur3.out", 0644);
  108.     fc4 = creat("../g/cur4.out", 0644);
  109.     fc5 = creat("../g/cur5.out", 0644);
  110.     fc6 = creat("../g/cur6.out", 0644);
  111.     write(fc1, cur1, idx * sizeof(double));
  112.     write(fc2, cur2, idx * sizeof(double));
  113.     write(fc3, cur3, idx * sizeof(double));
  114.     write(fc4, cur4, idx * sizeof(double));
  115.     write(fc5, cur5, idx * sizeof(double));
  116.     write(fc6, cur6, idx * sizeof(double));
  117.     exit(0);
  118. }
  119.  
  120.  
  121.  
  122.  
  123.  
  124. record()
  125. {
  126.     extern struct how how;
  127.     static unsigned short old[6];
  128.     double tor[6];
  129.     JNTS lod;
  130.  
  131.     int i;
  132.  
  133.     if (goalpos->scal < .2 || goalpos->scal > .9) {
  134.         for (i = 0; i < 6; ++i) {
  135.             old[i] = how.pos[i];
  136.         }
  137.         return;
  138.     }
  139.  
  140.     if (idx >= MAX) {
  141.         return;
  142.     }
  143.     cur1[idx] = how.adcr[0];
  144.     cur2[idx] = how.adcr[1];
  145.     cur3[idx] = how.adcr[2];
  146.     cur4[idx] = how.adcr[3];
  147.     cur5[idx] = how.adcr[4];
  148.     cur6[idx] = how.adcr[5];
  149.     adctotor(tor, how.adcr, how.pos, old);
  150.     gravload(&lod,
  151.     sncs.c2, sncs.c23, sncs.s23, sncs.c4, sncs.s4, sncs.c5, sncs.s5);
  152.  
  153.     tor1[idx] = tor[0] - lod.th1;
  154.     tor2[idx] = tor[1] - lod.th2;
  155.     tor3[idx] = tor[2] - lod.th3;
  156.     tor4[idx] = tor[3] - lod.th4;
  157.     tor5[idx] = tor[4] - lod.th5;
  158.     tor6[idx] = tor[5] - lod.th6;
  159.     cccc[idx] = outc;
  160.  
  161.     ++idx;
  162.  
  163.     for (i = 0; i < 6; ++i) {
  164.         old[i] = how.pos[i];
  165.     }
  166. }
  167.