home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 471 / rccl211 < prev    next >
Text File  |  1987-03-02  |  14KB  |  537 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.  
  9. #define CLR     '\032'
  10.  
  11. pumatask()
  12. {
  13.     int q;
  14.  
  15.     putchar(CLR);
  16.     printf("This is an implementation of the program given in Chap 5.\n");
  17.     printf("of Prof. Paul's book\n");
  18.     printf("Type 'y' to proceed");
  19.     QUERY(q);
  20.     if (q == 'y') {
  21.         book();
  22.     }
  23.     putchar(CLR);
  24.     printf("This program demonstrates that we can perform a task\n");
  25.     printf("namely, moving in loop to four different locations,\n");
  26.     printf("with respect to a moving coordinate frame.\n");
  27.     printf("The first part is performed in cartesian mode\n");
  28.     printf("in other words, the trajectory is constantly evaluated\n");
  29.     printf("with respect to the Cartesian world description\n");
  30.     printf("The second part is executed in joint mode,\n");
  31.     printf("this means that the succesive locations are computed\n");
  32.     printf("only at the beginning of each path segment, the coordinate\n");
  33.     printf("velocities estimated, and the intermediate locations\n");
  34.     printf("linearly interpolated at the joint level.\n");
  35.     printf("Type 'y' to proceed");
  36.     QUERY(q);
  37.     if (q == 'y') {
  38.         cvsj1();
  39.     }
  40.     putchar(CLR);
  41.     printf("This example is similar to the previous one.\n");
  42.     printf("Now the moving coordinate frame describes a circle\n");
  43.     printf("and the same move statement is executed four times.\n");
  44.     printf("This example clearly shows that the trajectories executed\n");
  45.     printf("in joint mode are aproximate\n");
  46.     printf("Type 'y' to proceed");
  47.     QUERY(q);
  48.     if (q == 'y') {
  49.         cvsj2();
  50.     }
  51.     putchar(CLR);
  52.     printf("This example again uses of moving coordinate frame.\n");
  53.     printf("We will instruct the robot to move along a path defined\n");
  54.     printf("by a spiral and a rotation\n");
  55.     printf("Type 'y' to proceed");
  56.     QUERY(q);
  57.     if (q == 'y') {
  58.         spire();
  59.     }
  60.     putchar(CLR);
  61.     printf("This example again uses of moving coordinate frame.\n");
  62.     printf("We will instruct the robot to move at separate locations\n");
  63.     printf("by changing asynchronously the value of a frame\n");
  64.     printf("as if this frame was updated by some external sensor\n");
  65.     printf("like a camera or a range finder.\n");
  66.     printf("Type 'y' to proceed");
  67.     QUERY(q);
  68.     if (q == 'y') {
  69.         psv();
  70.     }
  71.     putchar(CLR);
  72.     printf("This program makes use a very simple sensor to perform\n");
  73.     printf("a trajectory tracking on an arbitrary surface\n\n");
  74.     printf("The second part of the program uses the same sensor in a different way.
  75.     printf("Motions will be interrupted upon detection of an obstacle\n");
  76.     printf("When the robot senses an obtacle, it proceeds to the next\n");
  77.     printf("path segment.\n");
  78.     printf("Type 'y' to proceed");
  79.     QUERY(q);
  80.     if (q == 'y') {
  81.         ftrk();
  82.     }
  83.     putchar(CLR);
  84.     printf("This program shows the problems caused by the volume\n");
  85.     printf("swept when the arm changes it's configuration.\n");
  86.     printf("Type 'y' to proceed");
  87.     QUERY(q);
  88.     if (q == 'y') {
  89.         pcnf();
  90.     }
  91.     putchar(CLR);
  92. }
  93.  
  94.  
  95. book()
  96. {
  97.  
  98.     TRSF_PTR e, z, p, pa, pg, pd, h, ht, pha, pch, pac, pn;
  99.     POS_PTR pos1, pos2, pos3, pos4, pos5, pos6, pos7, pos8;
  100.     int q;
  101.  
  102.     z = gentr_eul("Z", 0., 0., 500., 0., 0., 0.);
  103.     e = gentr_eul("E", 0., 0., 200., 0., 0., 0.);
  104.     p    = gentr_eul("P"   , 200., 0., 0., -90., -90., 0.);
  105.     pa   = gentr_eul("PA"  , 30., 0., -50., 0., -45., 0.);
  106.     pg   = gentr_eul("PG"  , 0., 0., -50., 0., -45., 0.);
  107.     pd   = gentr_eul("PD"  , 50., 0., 0., 0., 0., 0.);
  108.     h    = gentr_eul("H"   , 250., 300., 0., 0., -90., 0.);
  109.     ht   = gentr_pao("HT"  , 100., 0., 50., 0., 1., 0., 0., 0., -1.);
  110.     pha  = gentr_eul("PHA" , 0., 0., -20., 0., 15., 0.);
  111.     pch  = gentr_eul("PCH" , 0., 0., 0., 0., 15., 0.);
  112.     pac  = gentr_eul("PAC" , 0., 0., 0., 0., 0., 0.);
  113.     pn   = gentr_eul("PN"  , 0., 0., 40., 0., 0., 0.);
  114.  
  115.     pos1 = makeposition("P1", z, t6, e, EQ, p, pa, TL, e);
  116.     pos2 = makeposition("P2", z, t6, e, EQ, p, pg, TL, e);
  117.     pos3 = makeposition("P3", z, t6, e, EQ, p, pd, pg, TL, pg);
  118.     pos4 = makeposition("P3", z, t6, e, EQ, h, ht, pha, pg, TL, pg);
  119.     pos5 = makeposition("P5", z, t6, e, EQ, h, ht, pch, pg, TL, pg);
  120.     pos6 = makeposition("P6", z, t6, e, EQ, h, ht, pac, pg, TL, pg);
  121.     pos7 = makeposition("P7", z, t6, e, EQ, h, ht, pn, pg, TL, pg);
  122.     pos8 = makeposition("P8", z, t6, e, EQ, h, ht, pn, pa, TL, e);
  123.  
  124.     for (; ; ) {
  125.         movejnts(pos1, 200, 2000);
  126.         movecart(pos2, 100, 700);
  127.         stop(0);
  128.         movecart(pos3, 100, 700);
  129.         movejnts(pos4, 100, 500);
  130.         movecart(pos5, 100, 500);
  131.         movejnts(pos6, 100, 700);
  132.         movecart(pos7, 100, 700);
  133.         stop(0);
  134.         movecart(pos8, 100, 700);
  135.         printf("more ?");  QUERY(q);
  136.         if (q == 'n')
  137.             break;
  138.     }
  139.     movejnts(park, 200, 2000);
  140. }
  141.  
  142.  
  143.  
  144. cvsj1()
  145. {
  146.     TRSF_PTR z, e , b1, s1, s2, s3, conv1, conv2;
  147.     POS_PTR  p0;
  148.     POS_PTR p1[4], p2[4];
  149.     int convfn1();
  150.     int i, j, q;
  151.  
  152.     conv1 = newtrans("CONV1",convfn1);
  153.     conv2 = newtrans("CONV2",convfn1);
  154.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
  155.     e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
  156.     b1 = gentr_rot("B1", 600. , -400., 500., yunit, 180.);
  157.     s1 = gentr_trsl("S1", 0., 0., 80.);
  158.     s2 = gentr_trsl("S2", 80., 0., 80.);
  159.     s3 = gentr_trsl("S3", 80., 0., 0.);
  160.  
  161.     p0 = makeposition("P0" , z, t6, e, EQ, b1, TL, e);
  162.     p1[0] = makeposition("P10" , z, t6, e, EQ, conv1, b1, TL, e);
  163.     p1[1] = makeposition("P11" , z, t6, e, EQ, conv1, b1, s1, TL, e);
  164.     p1[2] = makeposition("P12" , z, t6, e, EQ, conv1, b1, s2, TL, e);
  165.     p1[3] = makeposition("P13" , z, t6, e, EQ, conv1, b1, s3, TL, e);
  166.     p2[0] = makeposition("P20" , z, t6, e, EQ, conv2, b1, TL, e);
  167.     p2[1] = makeposition("P21" , z, t6, e, EQ, conv2, b1, s1, TL, e);
  168.     p2[2] = makeposition("P22" , z, t6, e, EQ, conv2, b1, s2, TL, e);
  169.     p2[3] = makeposition("P23" , z, t6, e, EQ, conv2, b1, s3, TL, e);
  170.  
  171.  
  172.     movejnts(p0, 100, 1500);
  173.     for (; ; ) {
  174.         waitfor(completed);
  175.         rtime = 0;
  176.         for (j = 0; j < 6; ++j) {
  177.             for (i = 0; i < 4; ++i) {
  178.                 movecart(p1[i], 100, 300);
  179.             }
  180.         }
  181.         movejnts(p0, 100, 1500);
  182.         waitfor(completed);
  183.         rtime = 0;
  184.         for (j = 0; j < 6; ++j) {
  185.             for (i = 0; i < 4; ++i) {
  186.                 movejnts(p2[i], 100, 300);
  187.             }
  188.         }
  189.         movejnts(p0, 100, 1500);
  190.         waitfor(completed);
  191.         printf("more ? ");
  192.         QUERY(q);
  193.         if (q == 'n') break;
  194.     }
  195.     movejnts(park, 100, 1500);
  196. }
  197.  
  198.  
  199. convfn1(t)
  200. TRSF_PTR t;
  201. {
  202.     t->p.y = .08 * rtime;
  203. }
  204.  
  205.  
  206.  
  207. cvsj2()
  208. {
  209.     TRSF_PTR z, e, conv, or, fl;
  210.     POS_PTR  pm, pf;
  211.     int convfn2();
  212.     int i;
  213.  
  214.     conv = newtrans("CONV",convfn2);
  215.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
  216.     e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
  217.     or = gentr_eul("OR", 000. , 600., 500.,    0., 0., 90.);
  218.     fl = gentr_rot("FL", 0. , 0., 0., yunit, 180.);
  219.  
  220.     pf = makeposition("PF" , z, t6, e, EQ, or, fl, TL, e);
  221.     pm = makeposition("PM" , z, t6, e, EQ, conv, or, fl, TL, e);
  222.  
  223.  
  224.     movejnts(pf, 300, 1500);
  225.     for (; ; ) {
  226.         printf("cart ");QUERY(i);
  227.         if (i == 'y') {
  228.             movecart(pm, 100, 800);
  229.             for (i = 0; i < 6; ++i) {
  230.                 movecart(pm, 100, 800);
  231.             }
  232.         }
  233.         movecart(pf, 100, 800);
  234.         printf("jnts ");QUERY(i);
  235.         if (i == 'y') {
  236.             movejnts(pm, 100, 800);
  237.             for (i = 0; i < 6; ++i) {
  238.                 movejnts(pm, 100, 800);
  239.             }
  240.         }
  241.         movecart(pf, 100, 800);
  242.         printf("quit ");QUERY(i);
  243.         if (i == 'y') {
  244.             break;
  245.         }
  246.         movecart(pf, 100, 800);
  247.     }
  248.     movecart(park, 300, 1500);
  249. }
  250.  
  251.  
  252.  
  253. convfn2(t)
  254. TRSF_PTR t;
  255. {
  256.     extern int rtime;
  257.     double time;
  258.  
  259.     double omega = .220;
  260.     double radius = 200.;
  261.  
  262.     time = rtime / 1000.;
  263.     t->p.x = radius * cos(omega * time * PIT2);
  264.     t->p.z = radius * sin(omega * time * PIT2);
  265. }
  266.  
  267.  
  268. spire()
  269. {
  270.     int spirefn();
  271.  
  272.     int q;
  273.  
  274.        TRSF_PTR
  275.     base = gentr_rot("BASE", -200., 100., 200., yunit, 0.),
  276.     tool = gentr_rot("TOOL", 0., 0., 180., yunit, 0.),
  277.     pin = gentr_rot("PIN", 400. , 300., 0., zunit, 0.),
  278.     pintip = gentr_rot("PINTIP", 0., 0., 50., yunit, 180.),
  279.     spire = newtrans("SPIRE", spirefn);
  280.  
  281.     POS_PTR
  282.     touch = makeposition("TOUCH",
  283.         base, t6, tool, EQ, pin, pintip, TL, tool),
  284.     spiral = makeposition("SPIRAL",
  285.         base, t6, tool, EQ, spire, pin, pintip, TL, tool);
  286.  
  287.     setvel(200, 100);
  288.  
  289.     for (; ; ) {
  290.         setmod('c');
  291.         move(touch);
  292.         QUERY(q); if (q == 'n') break;
  293.         setime(200, 15000);
  294.         move(spiral);
  295.     }
  296.     setmod('j');
  297.     move(park);
  298. }
  299.  
  300. /*
  301.  * generate a spiraling motion in the x y plan
  302.  */
  303.  
  304. spirefn(t)
  305. TRSF_PTR t;
  306. {
  307.     real radius = goalpos->scal * 50.;
  308.     real turns = 10.;
  309.     real heigth = 100.;
  310.     real angle = - 5.;
  311.     VECT k;
  312.  
  313.     t->p.x = radius * (k.x = cos(turns * goalpos->scal * PIT2));
  314.     t->p.y = radius * (k.y = sin(turns * goalpos->scal * PIT2));
  315.     t->p.z = heigth * goalpos->scal;
  316.     k.z = 0.;
  317.     Rot(t, &k, angle * goalpos->scal);
  318. }
  319.  
  320.  
  321.  
  322. psv()
  323. {
  324.     TRSF_PTR z, e , b1, conv;
  325.     POS_PTR  p1;
  326.     int convfo();
  327.     int q;
  328.     double iy;
  329.  
  330.     conv = newtrans("CONV",convfo);
  331.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
  332.     e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
  333.     b1 = gentr_rot("B1", 600. , 0., 500., yunit, 180.);
  334.  
  335.     b1->fn = hold;
  336.     p1 = makeposition("P1" , z, t6, e, EQ, conv, b1, TL, e);
  337.  
  338.     setmod('c');
  339.     setvel(300, 100);
  340.     move(p1);
  341.     for (; ; ) {
  342.         move(p1);
  343.         waitfor(completed);
  344.         if (p1->code == LIMIT) {
  345.             printf("limit reached\n");
  346.         }
  347.         printf("more "); QUERY(q); if (q == 'n') break;
  348.         printf("enter Z increment ");
  349.         scanf("%f", &iy);
  350.         b1->p.z += iy;
  351.     }
  352.     setmod('j');
  353.     move(park);
  354. }
  355.  
  356.  
  357. convfo(t)
  358. TRSF_PTR t;
  359. {
  360.     double time;
  361.  
  362.     time = rtime / 20000.;
  363.     t->p.y = 400. * cos(time * PIT2);
  364. }
  365.  
  366.  
  367. int sensor;
  368.  
  369. #define TOUCHED 10
  370.  
  371. ftrk()
  372. {
  373.     TRSF_PTR z, k1, k2, finf, abov;
  374.     TRSF_PTR b1, b2, b3, b4, b5, over, fing, getit, flip;
  375.     POS_PTR  q1, q2, got;
  376.     POS_PTR  p1, p2, p3, p4, p5, pp1, pp2, pp3, pp4, pp5, get;
  377.     int touchfn();
  378.     int fingfn();
  379.     int q;
  380.  
  381.     finf = newtrans("FINK",fingfn);
  382.     rot(finf, zunit, -90.);
  383.     fing = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
  384.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
  385.     k1 = gentr_rot("K1", 600. ,-300., 450., yunit, 180.);
  386.     k2 = gentr_rot("K2", 600. , 200., 450., yunit, 180.);
  387.     abov = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
  388.     b1 = gentr_trsl("B1", 600. ,-200., 300.);
  389.     b2 = gentr_trsl("B2", 600. ,-100., 300.);
  390.     b3 = gentr_trsl("B3", 600. , 000., 300.);
  391.     b4 = gentr_trsl("B4", 600. , 100., 300.);
  392.     b5 = gentr_trsl("B5", 600. , 200., 300.);
  393.     over = gentr_rot("OVER", 0., 0., 100., yunit, 180.);
  394.     flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);
  395.     getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);
  396.  
  397.  
  398.     q1 = makeposition("Q1" , z, t6, fing, EQ, k1, TL, fing);
  399.     q2 = makeposition("Q2" , z, t6, finf, EQ, k2, TL, finf);
  400.     got = makeposition("GOT", z, t6, EQ, abov, TL, t6);
  401.     p1 = makeposition("P1" , z, t6, fing, EQ, b1, over,TL , fing);
  402.     p2 = makeposition("P2" , z, t6, fing, EQ, b2, over,TL , fing);
  403.     p3 = makeposition("P3" , z, t6, fing, EQ, b3, over,TL , fing);
  404.     p4 = makeposition("P4" , z, t6, fing, EQ, b4, over,TL , fing);
  405.     p5 = makeposition("P5" , z, t6, fing, EQ, b5, over,TL , fing);
  406.     pp1 = makeposition("PP1" , z, t6, fing, EQ, b1, flip, TL , fing);
  407.     pp2 = makeposition("PP2" , z, t6, fing, EQ, b2, flip, TL , fing);
  408.     pp3 = makeposition("PP3" , z, t6, fing, EQ, b3, flip, TL , fing);
  409.     pp4 = makeposition("PP4" , z, t6, fing, EQ, b4, flip, TL , fing);
  410.     pp5 = makeposition("PP5" , z, t6, fing, EQ, b5, flip, TL , fing);
  411.     get = makeposition("GET", z, t6, EQ, getit, TL, t6);
  412.  
  413.  
  414.  
  415.     movejnts(get, 400, 1500);
  416.     waitfor(completed);
  417.     OPEN;
  418.     printf("put the sensor in the jaws ");
  419.     QUERY(q);
  420.     CLOSE;
  421.     printf("go ahead ");
  422.     QUERY(q);
  423.     if (q == 'n') {
  424.         movejnts(park, 400, 2500);
  425.         return;
  426.     }
  427.     sensor = adcopen(7);
  428.     for (; ; ) {
  429.         movecart(q1, 400, 1500);
  430.         stop(0);
  431.         sample(15);
  432.         movecart(q2, 200, 8000);
  433.         waitfor(completed);
  434.         sample(30);
  435.         QUERY(q); if (q == 'n') break;
  436.         movecart(got, 400, 1500);
  437.     }
  438.     movejnts(get, 400, 1500);
  439.     printf("type 'y' for poking");
  440.     QUERY(q);
  441.     if (q == 'n') {
  442.         movejnts(park, 400, 2500);
  443.         waitfor(completed);
  444.         OPEN;
  445.         return;
  446.     }
  447.     for (; ; ) {
  448.         movejnts(p1, 60, 1500);
  449.         evalfn(touchfn);
  450.         movejnts(pp1, 60, 1500);
  451.         movejnts(p2, 60, 1500);
  452.         evalfn(touchfn);
  453.         movejnts(pp2, 60, 1500);
  454.         movejnts(p3, 60, 1500);
  455.         evalfn(touchfn);
  456.         movejnts(pp3, 60, 1500);
  457.         movejnts(p4, 60, 1500);
  458.         evalfn(touchfn);
  459.         movejnts(pp4, 60, 1500);
  460.         movejnts(p5, 60, 1500);
  461.         evalfn(touchfn);
  462.         movejnts(pp5, 60, 1500);
  463.         movecart(get, 60, 1500);
  464.         waitfor(pp1->end)
  465.         if (pp1->code == TOUCHED) printf("touched\n");
  466.         else printf("not touched\n");
  467.         waitfor(pp2->end)
  468.         if (pp2->code == TOUCHED) printf("touched\n");
  469.         else printf("not touched\n");
  470.         waitfor(pp3->end)
  471.         if (pp3->code == TOUCHED) printf("touched\n");
  472.         else printf("not touched\n");
  473.         waitfor(pp4->end)
  474.         if (pp4->code == TOUCHED) printf("touched\n");
  475.         else printf("not touched\n");
  476.         waitfor(pp5->end)
  477.         if (pp5->code == TOUCHED) printf("touched\n");
  478.         else printf("not touched\n");
  479.  
  480.         printf("more ? ");
  481.         QUERY(q); if (q == 'n') break;
  482.     }
  483.     movejnts(park, 400, 1500);
  484.     waitfor(completed);
  485.     OPEN;
  486. }
  487.  
  488.  
  489. touchfn()
  490. {
  491.     if (how.adcr[sensor] > 1) {
  492.         nextmove = TOUCHED;
  493.     }
  494. }
  495.  
  496.  
  497. fingfn(t)
  498. TRSF_PTR t;
  499. {
  500.     t->p.z += (how.adcr[sensor] * .01 - 3.) / 3.;
  501. }
  502.  
  503.  
  504.  
  505. pcnf()
  506. {
  507.     TRSF_PTR z, e , b1, rt;
  508.     POS_PTR  p1, p2;
  509.     int q;
  510.  
  511.     z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
  512.     e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
  513.     b1 = gentr_pao("B1", -128. , 800.,  900., 0., 0., -1., 0., 1., 0.);
  514.     rt = rot(newtrans("RT", const), xunit, 90.);
  515.  
  516.     p1   = makeposition("P1" , z, t6, e, EQ, b1, TL, e);
  517.     p2   = makeposition("P2" , z, t6, e, rt, EQ, b1, TL, e);
  518.  
  519.  
  520.     movejnts(p1, 300, 2000);
  521.     for (; ; ) {
  522.         printf("the arm is lun, goes luf ? ");
  523.         QUERY(q); if (q == 'n') break;
  524.         moveconf(p2, 300, 1000, "f");
  525.         printf("the arm is luf, goes ldn ? ");
  526.         QUERY(q); if (q == 'n') break;
  527.         moveconf(p2, 300, 2000, "dn");
  528.         printf("the arm is ldn, goes lun ? ");
  529.         QUERY(q); if (q == 'n') break;
  530.         moveconf(p1, 300, 2000, "u");
  531.     }
  532.     moveconf(park, 300, 2000, "lun");
  533.     printf("END\n");
  534. }
  535.  
  536.  
  537.