home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL025 < prev    next >
Encoding:
Text File  |  1987-03-02  |  11.2 KB  |  600 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : teach.c
  7.  *      Remarks : Manual teach mode, uses the rest of the system.
  8.  *      Usage   : part of the library
  9.  */
  10.  
  11. /*LINTLIBRARY*/
  12.  
  13. #include <ctype.h>
  14. #include "../h/rccl.h"
  15. #include "../h/hand.h"
  16. #include "../h/which.h"
  17. #include "../h/kine.h"
  18. #include "../h/umac.h"
  19.  
  20.  
  21. /*
  22.  * the parse function put all user's requests in those statics
  23.  */
  24.  
  25. static TRSF wtct, wrct, ttct, trct;             /* static alloc of transfor */
  26. static TRSF_PTR wtc = &wtct,                    /* world translation change */
  27.         wrc = &wrct,                    /* ..... rotation    ...... */
  28.         ttc = &ttct,                    /* tool  translation ...... */
  29.         trc = &trct;                    /* ....  rotation    ...... */
  30. static TRSF_PTR e;                              /* tool transform           */
  31. static int vts, vrs;                            /* velocities               */
  32. static char *doom;                              /* message string pointer   */
  33. static double flx, fly, flz, tlx, tly, tlz;     /* force limit values       */
  34. static int flimp;                               /* limit on / off           */
  35. static real object;                             /* mass of object           */
  36.  
  37.  
  38. teach(t, p) /*::*/
  39. POS_PTR p;
  40. TRSF_PTR t;
  41. {
  42.     POS_PTR teach;                  /* the pos equations    */
  43.     TRSF_PTR tea;                   /* T6 E = TEA           */
  44.     TRSF wtt,
  45.          wrt,                       /* dyn. alloc.          */
  46.          ttt,
  47.          trt;
  48.     TRSF_PTR wt = &wtt,             /* world transl. part of tea    */
  49.          wr = &wrt,             /* ..... rot. ..............    */
  50.          tt = &ttt,             /* tool  transl. part of tea    */
  51.          tr = &trt;             /* ....  rot. ..............    */
  52.     TRSF temp;
  53.     int action,                     /* returned by parse            */
  54.         q;
  55.     bool recorded = NO;             /* set when recorded            */
  56.     double getreal(), atof();
  57.     int check();
  58.  
  59.     if (fddb > 0) {
  60.         waitfor(completed);
  61.         if (gettr(t, fddb) >= 0) {      /* we've got it         */
  62.             return(YES);
  63.         }
  64.     }
  65.  
  66.     e = gentr_trsl("E", 0., 0., 170.);      /* make a tool  */
  67.     e->fn = hold;                           /* hold it      */
  68.     tea = newtrans("TEA", hold);            /* make tea     */
  69.  
  70.     teach = makeposition("TEACH", t6, e, EQ, tea, TL, e);
  71.  
  72.     stop(100);
  73.     waitfor(completed);
  74.     printf("teach mode V1.0, transform %s, position %s\n",
  75.         t->name, p->name);
  76.     setmod('c');
  77.     action = 'n';
  78.     vts = 100;
  79.     vrs = 10;
  80.     doom = "";
  81.     flx = fly = flz = tlx = tly = tlz = 0.;
  82.     flimp = NO;
  83.     object = 0.;
  84.     for (; ; ) {
  85.         waitfor(completed);
  86.         Assigntr(wt, unitr);
  87.         Assigntr(wr, unitr);
  88.         Assigntr(tt, unitr);
  89.         Assigntr(tr, unitr);
  90.  
  91.         Trmult(&temp, t6, e);
  92.         Taketrsl(wt, &temp);
  93.         Takerot(tr, &temp);
  94.         if (action == 'y') {
  95.             Trmultinp(wt, wtc);
  96.             Trmultinp(wr, wrc);
  97.             Trmultinp(tt, ttc);
  98.             Trmultinp(tr, trc);
  99.         }
  100.         if (action == 'q') {
  101.             if (!recorded) {
  102.                 printf("nothing taught, exit ? ");
  103.                 QUERY(q);
  104.                 if (q == 'y') {
  105.                     break;
  106.                 }
  107.             }
  108.             else {
  109.                 break;
  110.             }
  111.         }
  112.  
  113.         if (action == 'x') {
  114.             recorded = NO;
  115.             break;
  116.         }
  117.         Trmult(tea, wt, wr);
  118.         Trmult(&temp, tr, tt);
  119.         Trmultinp(tea, &temp);
  120.         setvel(vts, vrs);
  121.         evalfn(check);
  122.         if (flimp) {
  123.             limit("fx fy fz tx ty tz", flx, fly, flz, tlx, tly, tlz);
  124.         }
  125.         massis(object);
  126.         move(teach);
  127.         if (action == 'r') {
  128.             update(t, p);
  129.             recorded = YES;
  130.         }
  131.         if (action == 's') {
  132.             if (fddb < 0) {
  133.                 printf("no data base specified\n");
  134.             }
  135.             else {
  136.                 if (!recorded) {
  137.                     printf("nothing recorded\n");
  138.                 }
  139.                 else {
  140.                     if (savetr(t, fddb) < 0) {
  141.                         recorded = NO;
  142.                         break;
  143.                     }
  144.                 }
  145.             }
  146.         }
  147.         move(there);
  148.         Assigntr(wtc, unitr);
  149.         Assigntr(wrc, unitr);
  150.         Assigntr(ttc, unitr);
  151.         Assigntr(trc, unitr);
  152.         printf("%s", doom);
  153.         if (teach->code == ONF) {
  154.             printf("stopped on force\n");
  155.         }
  156.  
  157.         while ((action = parse()) == 'e' ||
  158.             action == 'S' || action == 'p') {
  159.             if (action == 'S') {
  160.                 nextmove = YES;
  161.                 printf("%s", doom);
  162.                 if (teach->code == ONF) {
  163.                     printf("stopped on force\n");
  164.                 }
  165.                 printf(">> stopped\n");
  166.             }
  167.             if (action == 'p') {
  168.                 display();
  169.             }
  170.         }
  171.     }
  172.     move(there);
  173.     waitfor(completed);
  174.     freeposition(teach);
  175.     freetrans(tea);
  176.     freetrans(e);
  177.     return(recorded);
  178. }
  179.  
  180.  
  181. /*
  182.  * check for joint limits
  183.  */
  184.  
  185. #define BOUND   10. * DEGTORAD
  186. #define SAFEV   20.
  187.  
  188. static check() /*##*/
  189. {
  190.     real bound = BOUND;
  191.  
  192.     if (*doom == '\0') {
  193.         if (j6->th1 < bound || j6->th1 > jrng_c.th1 - bound ||
  194.             j6->th2 < bound || j6->th2 > jrng_c.th2 - bound ||
  195.             j6->th3 < bound || j6->th3 > jrng_c.th3 - bound ||
  196.             j6->th4 < bound || j6->th4 > jrng_c.th4 - bound ||
  197.             j6->th5 < bound || j6->th5 > jrng_c.th5 - bound ||
  198.             j6->th6 < bound || j6->th6 > jrng_c.th6 - bound) {
  199.             nextmove = YES;
  200.             doom = "next to limit(s)\n";
  201.         }
  202.     }
  203.     else {
  204.         doom = "";
  205.     }
  206.     if (FABS(jd->th1 * SAFEV) > jmxv_c.th1 ||
  207.         FABS(jd->th2 * SAFEV) > jmxv_c.th2 ||
  208.         FABS(jd->th3 * SAFEV) > jmxv_c.th3 ||
  209.         FABS(jd->th4 * SAFEV) > jmxv_c.th4 ||
  210.         FABS(jd->th5 * SAFEV) > jmxv_c.th5 ||
  211.         FABS(jd->th6 * SAFEV) > jmxv_c.th6) {
  212.         nextmove = YES;
  213.         doom = "not so fast\n";
  214.     }
  215. }
  216.  
  217.  
  218.  
  219. /*
  220.  * display current settings
  221.  */
  222.  
  223. static display() /*##*/
  224. {
  225.     printf("T6:\n");
  226.     printr(t6, stdout);
  227.     printf("E:\n");
  228.     printr(e, stdout);
  229.     printf("veloc t:%d r:%d\n", vts, vrs);
  230.     dial(j6->th1, jrng_c.th1, '1');
  231.     dial(j6->th2, jrng_c.th2, '2');
  232.     dial(j6->th3, jrng_c.th3, '3');
  233.     dial(j6->th4, jrng_c.th4, '4');
  234.     dial(j6->th5, jrng_c.th5, '5');
  235.     dial(j6->th6, jrng_c.th6, '6');
  236.     putchar('\n');
  237.     if (flimp) {
  238.         printf(
  239. "force limits :fx %-5.2f  fy %-5.2f  fz %-5.2f   fX %-5.2f  fY %-5.2f  fZ %-5.2f
  240.         flx, fly, flz, tlx, tly, tlz);
  241.     }
  242.     else {
  243.         printf("no force limit\n");
  244.     }
  245.     printf("mass of object : %f kg\n", object);
  246. }
  247.  
  248. #define WIDTH 12
  249.  
  250. /*
  251.  * display joint positions within their ranges
  252.  */
  253.  
  254. static dial(v, m, c) /*##*/
  255. real v, m;
  256. int c;
  257. {
  258.     int i, p;
  259.  
  260.     v -= BOUND;
  261.     m -= BOUND + BOUND;
  262.     p = (v / m) * (WIDTH - 1);
  263.  
  264.     for (i = 0; i < WIDTH; ++i) {
  265.         putchar((i == p) ? c : '_');
  266.     }
  267.     putchar(' ');
  268. }
  269.  
  270.  
  271. /*
  272.  * analyze command line, return world and tool modifications.
  273.  * return :
  274.  *              S       stop
  275.  *              q       quit
  276.  *              x       forced exit
  277.  *              r       record
  278.  *              p       print
  279.  *              s       save
  280.  *              e       error
  281.  *              n       do nothing
  282.  *              y       parse ok, intructions in globals
  283.  */
  284.  
  285. static char coml[80], *c;
  286.  
  287. static parse() /*##*/
  288. {
  289.     putchar('?');
  290.  (void) gets(coml);
  291.     if (strlen(coml) == 0) {
  292.         return('S');
  293.     }
  294.     for (c = coml; *c != '\0';) {
  295.         while (*c != '\0' && isspace(*c)) {
  296.             ++c;
  297.         }
  298.         switch (*c) {
  299.         case 'q' :
  300.             ++c;
  301.             if (*c == '!') {
  302.                 return('x');
  303.             }
  304.             else {
  305.                 return('q');
  306.             }
  307.  
  308.         case 'r' :
  309.             return('r');
  310.  
  311.         case 'p' :
  312.             return('p');
  313.  
  314.         case 's' :
  315.             return('s');
  316.  
  317.         case 'o' :
  318.             ++c;
  319.             OPEN;
  320.             break;
  321.  
  322.         case 'c' :
  323.             ++c;
  324.             CLOSE;
  325.             break;
  326.  
  327.         case 'w' :
  328.             switch (*++c) {
  329.             case 'x' :
  330.                 Trslm(wtc, getreal(), 0., 0.);
  331.                 break;
  332.  
  333.             case 'y' :
  334.                 Trslm(wtc, 0., getreal(), 0.);
  335.                 break;
  336.  
  337.             case 'z' :
  338.                 Trslm(wtc, 0., 0., getreal());
  339.                 break;
  340.  
  341.             case 'X' :
  342.                 Rotm(wrc, xunit, getreal());
  343.                 break;
  344.  
  345.             case 'Y' :
  346.                 Rotm(wrc, yunit, getreal());
  347.                 break;
  348.  
  349.             case 'Z' :
  350.                 Rotm(wrc, zunit, getreal());
  351.                 break;
  352.  
  353.             default :
  354.                 err();
  355.                 return('e');
  356.             }
  357.             break;
  358.  
  359.         case 't' :
  360.             switch (*++c) {
  361.             case 'x' :
  362.                 Trslm(ttc, getreal(), 0., 0.);
  363.                 break;
  364.  
  365.             case 'y' :
  366.                 Trslm(ttc, 0., getreal(), 0.);
  367.                 break;
  368.  
  369.             case 'z' :
  370.                 Trslm(ttc, 0., 0., getreal());
  371.                 break;
  372.  
  373.             case 'X' :
  374.                 Rotm(trc, xunit, getreal());
  375.                 break;
  376.  
  377.             case 'Y' :
  378.                 Rotm(trc, yunit, getreal());
  379.                 break;
  380.  
  381.             case 'Z' :
  382.                 Rotm(trc, zunit, getreal());
  383.                 break;
  384.  
  385.             default :
  386.                 err();
  387.                 return('e');
  388.             }
  389.             break;
  390.  
  391.         case 'e' :
  392.             switch (*++c) {
  393.             case 'x' :
  394.                 Trslm(e, getreal(), 0., 0.);
  395.                 break;
  396.  
  397.             case 'y' :
  398.                 Trslm(e, 0., getreal(), 0.);
  399.                 break;
  400.  
  401.             case 'z' :
  402.                 Trslm(e, 0., 0., getreal());
  403.                 break;
  404.  
  405.             case 'X' :
  406.                 Rotm(e, xunit, getreal());
  407.                 break;
  408.  
  409.             case 'Y' :
  410.                 Rotm(e, yunit, getreal());
  411.                 break;
  412.  
  413.             case 'Z' :
  414.                 Rotm(e, zunit, getreal());
  415.                 break;
  416.  
  417.             default :
  418.                 err();
  419.                 return('e');
  420.             }
  421.             break;
  422.  
  423.         case 'f' :
  424.             switch (*++c) {
  425.             case 'x' :
  426.                 flx = getreal();
  427.                 break;
  428.  
  429.             case 'y' :
  430.                 fly = getreal();
  431.                 break;
  432.  
  433.             case 'z' :
  434.                 flz = getreal();
  435.                 break;
  436.  
  437.             case 'X' :
  438.                 tlx = getreal();
  439.                 break;
  440.  
  441.             case 'Y' :
  442.                 tly = getreal();
  443.                 break;
  444.  
  445.             case 'Z' :
  446.                 tlz = getreal();
  447.                 break;
  448.  
  449.             default :
  450.                 err();
  451.                 return('e');
  452.             }
  453.             break;
  454.  
  455.         case 'l' :
  456.             flimp = !flimp;
  457.             return('n');
  458.  
  459.         case 'm' :
  460.             object = getreal();
  461.             return('n');
  462.  
  463.         case 'v' :
  464.             vts = getreal();
  465.             vrs = getreal();
  466.             return('n');
  467.  
  468.         case '?' :
  469.             help();
  470.             return('n');
  471.  
  472.         case '\0':
  473.             break;
  474.  
  475.         default :
  476.             err();
  477.             return('e');
  478.         }
  479.     }
  480.     return('y');
  481. }
  482.  
  483.  
  484. /*
  485.  *                  -------        -< d >-
  486.  *                \|       |     \|       |
  487.  *                 --< d >---< . >---------
  488.  *       -< + >-  |        |              |
  489.  *      |       | |         --------------|/
  490.  *   >--------------< . >----< d >-------------->
  491.  *      |       |          |\      |
  492.  *       -< - >-            -------
  493.  */
  494.  
  495. static double getreal() /*##*/
  496. {
  497.     char *f;
  498.     double v;
  499.  
  500.     ++c;
  501.     while (isspace(*c)) {
  502.         ++c;
  503.     }
  504.     f = c;
  505.     switch (*c) {
  506.     case '\0' :
  507.         err();
  508.         return(0.);
  509.  
  510.     case '+' :
  511.         ++c;
  512.         break;
  513.  
  514.     case '-' :
  515.         ++c;
  516.         break;
  517.     }
  518.     switch (*c) {
  519.     case '\0' :
  520.         err();
  521.         return(0.);
  522.  
  523.     case '.' :
  524.         ++c;
  525.         if (!isdigit(*c)) {
  526.             err();
  527.             return(0.);
  528.         }
  529.         while (isdigit(*c)) {
  530.             ++c;
  531.         }
  532.         break;
  533.  
  534.     default :
  535.         if (!isdigit(*c)) {
  536.             err();
  537.             return(0.);
  538.         }
  539.         while (isdigit(*c)) {
  540.             ++c;
  541.         }
  542.         if (*c == '.') {
  543.             ++c;
  544.             while(isdigit(*c)) {
  545.                 ++c;
  546.             }
  547.         }
  548.         break;
  549.     }
  550.     v = atof(f);
  551.     if (FABS(v) > 10000) {
  552.         err();
  553.         v = 0.;
  554.     }
  555.     return(v);
  556. }
  557.  
  558.  
  559.  
  560. static err() /*##*/
  561. {
  562.     int n;
  563.  
  564.     *c = '\0';
  565.     n = strlen(coml) + 1;
  566.     while (n--) {
  567.         putchar(' ');
  568.     }
  569.     putchar('^');
  570.     putchar('\n');
  571. }
  572.  
  573.  
  574.  
  575. static help() /*##*/
  576. {
  577.     static char msg[] =
  578. "\
  579. These commands are executed one per line:\n\
  580. \t<return>\tinterrupt arm motion\n\
  581. \tq\t\tquit\n\
  582. \tq!\t\tquit, ignore not recorded\n\
  583. \tr\t\trecord transform\n\
  584. \tp\t\tdisplay current settings\n\
  585. \ts\t\tsave transform on data base\n\
  586. \tl\t\ttoggle force monitor\n\
  587. \tv<t r>\t\tset velocities\n\
  588. \tm<m>\t\tset mass of object\n\
  589. \t?\t\tthis message\n\
  590. These commands cumulate:\n\
  591. \to\t\t\topen hand\n\
  592. \tc\t\t\tclose hand\n\
  593. \tw<x/y/z/X/Y/Z><k>\tmove world coordinates\n\
  594. \tt<x/y/z/X/Y/Z><k>\tmove tool coordinates\n\
  595. \te<x/y/z/X/Y/Z><k>\tchange tool transform\n\
  596. \tf<x/y/z/X/Y/Z><k>\tset force limits\n\
  597. ";
  598.     printf("%s", msg);
  599. }
  600.