home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL039 < prev    next >
Encoding:
Text File  |  1987-03-02  |  11.3 KB  |  523 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : misc.c
  7.  *      Remarks : Do all the io. Very dependent on the version.
  8.  *                Also low level interface.
  9.  *      Usage   : part of the library
  10.  */
  11.  
  12. #include <signal.h>
  13. #include "../h/which.h"
  14. #include "../h/switch.h"
  15. #include "../h/rccl.h"
  16. #include "../h/manip.h"
  17. #include "../h/kine.h"
  18. #include "../h/bio.h"
  19. #include "../h/umac.h"
  20.  
  21.  
  22. #ifdef REAL
  23. #include "../h/rtc.h"
  24.  
  25. extern struct chg chg;                  /* in rtc                       */
  26. extern struct how how;                  /* .. ...                       */
  27.  
  28. static unsigned short old[NJOINTS] =    /* previous encoder values      */
  29.         {0, 0, 0, 0, 0, 0};
  30. static JNTS lod;                        /* gravity loads                */
  31.  
  32.  
  33. /*
  34.  * obtain observed joint torques.
  35.  * if velocity small : no torque, otherwise we get very erroneous results
  36.  * remove gravity loads
  37.  */
  38.  
  39. getobst_n(j) /*::*/
  40. register JNTS_PTR j;
  41. {
  42.     double t[NJOINTS];              /* observed torques             */
  43.  
  44.     adctotor(t, how.adcr, how.pos, old);
  45.     if (ABS((short)(how.pos[J1] - old[J1])) < 1) {
  46.         j->th1 = 0.;
  47.     }
  48.     else {
  49.         j->th1 = lod.th1 - t[J1];
  50.     }
  51.     if (ABS((short)(how.pos[J2] - old[J2])) < 1) {
  52.         j->th2 = 0.;
  53.     }
  54.     else {
  55.         j->th2 = lod.th2 - t[J2];
  56.     }
  57.     if (ABS((short)(how.pos[J3] - old[J3])) < 1) {
  58.         j->th3 = 0.;
  59.     }
  60.     else {
  61.         j->th3 = lod.th3 - t[J3];
  62.     }
  63.     if (ABS((short)(how.pos[J4] - old[J4])) < 1) {
  64.         j->th4 = 0.;
  65.     }
  66.     else {
  67.         j->th4 = lod.th4 - t[J4];
  68.     }
  69.     if (ABS((short)(how.pos[J5] - old[J5])) < 1) {
  70.         j->th5 = 0.;
  71.     }
  72.     else {
  73.         j->th5 = lod.th5 - t[J5];
  74.     }
  75.     if (ABS((short)(how.pos[J6] - old[J6])) < 1) {
  76.         j->th6 = 0.;
  77.     }
  78.     else {
  79.         j->th6 = lod.th6 - t[J6];
  80.     }
  81. }
  82.  
  83.  
  84. /*
  85.  * obtain observed joint pos
  86.  */
  87.  
  88. getobsj_n(j) /*::*/
  89. register JNTS_PTR j;
  90. {
  91.     double r[NJOINTS];
  92.  
  93.     enctorng(r, how.pos);
  94.     j->th1 = r[J1];
  95.     j->th2 = r[J2];
  96.     j->th3 = r[J3];
  97.     j->th4 = r[J4];
  98.     j->th5 = r[J5];
  99.     j->th6 = r[J6];
  100. }
  101.  
  102.  
  103. /*
  104.  * sort out the requests of setpoint and translate them for rtc
  105.  * add gravity loads
  106.  * returns 'LIMIT' if next to limits
  107.  */
  108.  
  109. jnsend_n(j, s, cpy, t) /*:: must be called the last */
  110. register JNTS_PTR j;
  111. register int s;
  112. register int cpy;
  113. register JNTS_PTR t;
  114. {
  115.     double jset[NJOINTS];           /* desired setpoint     */
  116.     double jtor[NJOINTS];           /* desired torques      */
  117.     unsigned short enc[NJOINTS];    /* encoder values       */
  118.     short cur[NJOINTS];             /* current values       */
  119.     int code = OK;
  120.  
  121. #ifdef PUMA
  122.     gravload_n(&lod,
  123. sncs_d.c2, sncs_d.c23, sncs_d.s23, sncs_d.c4, sncs_d.s4, sncs_d.c5, sncs_d.s5);
  124. #endif
  125. #ifdef STAN
  126.     gravload_n(&lod,
  127. sncs_d.s2, sncs_d.c2, sncs_d.d3, sncs_d.c4, sncs_d.s4, sncs_d.c5, sncs_d.s5);
  128. #endif
  129.  
  130.     jset[J1] = j->th1;
  131.     jset[J2] = j->th2;
  132.     jset[J3] = j->th3;
  133.     jset[J4] = j->th4;
  134.     jset[J5] = j->th5;
  135.     jset[J6] = j->th6;
  136.  
  137.     if (rngtoenc(enc, jset)) {
  138.         code = LIMIT;
  139.     }
  140.  
  141.     jtor[J1] = t->th1 + lod.th1;
  142.     jtor[J2] = t->th2 + lod.th2;
  143.     jtor[J3] = t->th3 + lod.th3;
  144.     jtor[J4] = t->th4 + lod.th4;
  145.     jtor[J5] = t->th5 + lod.th5;
  146.     jtor[J6] = t->th6 + lod.th6;
  147.  
  148.     tortodac(cur, jtor, how.pos, old);
  149.  
  150.     if (cpy & SELJ1) {
  151.         chg.i_motion[J1].vali = cur[J1];
  152.         chg.i_motion[J1].set = CUR;
  153.     }
  154.     else {
  155.         chg.i_motion[J1].vali = enc[J1];
  156.         chg.i_motion[J1].set = POS;
  157.     }
  158.     if (cpy & SELJ2) {
  159.         chg.i_motion[J2].vali = cur[J2];
  160.         chg.i_motion[J2].set = CUR;
  161.     }
  162.     else {
  163.         chg.i_motion[J2].vali = enc[J2];
  164.         chg.i_motion[J2].set = POS;
  165.     }
  166.     if (cpy & SELJ3) {
  167.         chg.i_motion[J3].vali = cur[J3];
  168.         chg.i_motion[J3].set = CUR;
  169.     }
  170.     else {
  171.         chg.i_motion[J3].vali = enc[J3];
  172.         chg.i_motion[J3].set = POS;
  173.     }
  174.     if (cpy & SELJ4) {
  175.         chg.i_motion[J4].vali = cur[J4];
  176.         chg.i_motion[J4].set = CUR;
  177.     }
  178.     else {
  179.         chg.i_motion[J4].vali = enc[J4];
  180.         chg.i_motion[J4].set = POS;
  181.     }
  182.     if (cpy & SELJ5) {
  183.         chg.i_motion[J5].vali = cur[J5];
  184.         chg.i_motion[J5].set = CUR;
  185.     }
  186.     else {
  187.         chg.i_motion[J5].vali = enc[J5];
  188.         chg.i_motion[J5].set = POS;
  189.     }
  190.     if (cpy & SELJ6) {
  191.         chg.i_motion[J6].vali = cur[J6];
  192.         chg.i_motion[J6].set = CUR;
  193.     }
  194.     else {
  195.         chg.i_motion[J6].vali = enc[J6];
  196.         chg.i_motion[J6].set = POS;
  197.     }
  198.  
  199.     if (hdpos != 0) {
  200.         chg.g_hand.valg = hdpos;
  201.         chg.g_hand.set = YES;
  202.         hdpos = 0;
  203.     }
  204.     if (s != 0) {
  205.         double fact = s / DEF_SAMPLE;
  206.         register int n, r;
  207.  
  208.         jmxv_c.th1 *= fact;
  209.         jmxv_c.th2 *= fact;
  210.         jmxv_c.th3 *= fact;
  211.         jmxv_c.th4 *= fact;
  212.         jmxv_c.th5 *= fact;
  213.         jmxv_c.th6 *= fact;
  214.         n = s / HARDCLOCK;
  215.         r = 0;
  216.         while ((n >>= 1) != 0) {
  217.             ++r;
  218.         }
  219.         chg.g_rate.valg = r;
  220.         chg.g_rate.set = YES;
  221.     }
  222.  
  223.     old[J1] = how.pos[J1];
  224.     old[J2] = how.pos[J2];
  225.     old[J3] = how.pos[J3];
  226.     old[J4] = how.pos[J4];
  227.     old[J5] = how.pos[J5];
  228.     old[J6] = how.pos[J6];
  229.  
  230.     return(code);
  231. }
  232. #endif
  233.  
  234.  
  235. #ifndef REAL            /* display facilities */
  236.  
  237. static putenco(enc, j) /*##*/
  238. register unsigned short *enc;
  239. JNTS_PTR j;
  240. {
  241.     double jset[NJOINTS];
  242.  
  243.     jset[J1] = j->th1;
  244.     jset[J2] = j->th2;
  245.     jset[J3] = j->th3;
  246.     jset[J4] = j->th4;
  247.     jset[J5] = j->th5;
  248.     jset[J6] = j->th6;
  249.  (void) rngtoenc(enc, jset);
  250.     enc[6] = hdpos << 4;            /* that's for play      */
  251. }
  252.  
  253.  
  254. /*
  255.  * simulation cannot do anything
  256.  */
  257.  
  258. getobst_n(j) /*::*/
  259. JNTS_PTR j;
  260. {
  261.     j->th1 = 0.;
  262.     j->th2 = 0.;
  263.     j->th3 = 0.;
  264.     j->th4 = 0.;
  265.     j->th5 = 0.;
  266.     j->th6 = 0.;
  267. }
  268.  
  269.  
  270. /*
  271.  * simulates comply by blocking the joints
  272.  */
  273.  
  274. static JNTS simulcpyj;
  275.  
  276. getobsj_n(j) /*::*/
  277. JNTS_PTR j;
  278. {
  279.     assignjs_n(j, &simulcpyj);
  280. }
  281.  
  282.  
  283. /*
  284.  * catches the INT signal
  285.  */
  286.  
  287. static enough() /*##*/
  288. {
  289.     mess = "Interrupt";
  290.     terminate = YES;
  291. }
  292.  
  293.  
  294. /*
  295.  * displays t6 and joints
  296.  */
  297.  
  298. #define BOUND   5. * DEGTORAD
  299.  
  300. jnsend_n(p, j, jl, t, ts, tm, cpyj, mt) /*::*/
  301. PST_PTR p;
  302. JNTS_PTR j, jl;
  303. TRSF_PTR t;
  304. int ts, tm;
  305. int cpyj;
  306. char mt;
  307. {
  308.     unsigned short enco[7];
  309.     int code = OK;
  310.     real bound = BOUND;
  311.  
  312.  (void) signal(SIGINT, enough);
  313.  
  314.     if (!(cpyj & SELJ1)) {
  315.         simulcpyj.th1 = j->th1;
  316.     }
  317.     if (!(cpyj & SELJ2)) {
  318.         simulcpyj.th2 = j->th2;
  319.     }
  320.     if (!(cpyj & SELJ3)) {
  321.         simulcpyj.th3 = j->th3;
  322.     }
  323.     if (!(cpyj & SELJ4)) {
  324.         simulcpyj.th4 = j->th4;
  325.     }
  326.     if (!(cpyj & SELJ5)) {
  327.         simulcpyj.th5 = j->th5;
  328.     }
  329.     if (!(cpyj & SELJ6)) {
  330.         simulcpyj.th6 = j->th6;
  331.     }
  332.  
  333.     if (fabs(j->th1 - jl->th1) > jmxv_c.th1 ||
  334.         fabs(j->th2 - jl->th2) > jmxv_c.th2 ||
  335.         fabs(j->th3 - jl->th3) > jmxv_c.th3 ||
  336.         fabs(j->th4 - jl->th4) > jmxv_c.th4 ||
  337.         fabs(j->th5 - jl->th5) > jmxv_c.th5 ||
  338.         fabs(j->th6 - jl->th6) > jmxv_c.th6) {
  339.         fprintf(stderr, "glitch %d\n", rtime);
  340.     }
  341.     if (j->th1 < bound || j->th1 > jrng_c.th1 - bound ||
  342.         j->th2 < bound || j->th2 > jrng_c.th2 - bound ||
  343.         j->th3 < bound || j->th3 > jrng_c.th3 - bound ||
  344.         j->th4 < bound || j->th4 > jrng_c.th4 - bound ||
  345.         j->th5 < bound || j->th5 > jrng_c.th5 - bound ||
  346.         j->th6 < bound || j->th6 > jrng_c.th6 - bound) {
  347.         fprintf(stderr, "limit  %d\n", rtime);
  348.         code = LIMIT;
  349.     }
  350.  
  351.     if (opsw_n.encoders) {
  352.         putenco(enco, j);
  353.         Write(&iobf_n[EB], (char *)enco, sizeof(enco));
  354.     }
  355.     if (opsw_n.t6butnotj6) {
  356.         if (opsw_n.graphics)
  357.             outmat(t, tm, mt);
  358.         if (opsw_n.numerics)
  359.             prtmat(p, t, ts, tm, mt, fpo);
  360.     }
  361.     else {
  362.         if (opsw_n.graphics)
  363.             outjns(&simulcpyj, tm, mt);
  364.         if (opsw_n.numerics)
  365.             prtjns(p, &simulcpyj, ts, tm, mt, cpyj, fpo);
  366.     }
  367.  (void) signal(SIGINT, release_l);
  368.     return(code);
  369. }
  370.  
  371. /*
  372.  * output t6 in a buffered manner
  373.  */
  374.  
  375. static outmat(t, tm, mt) /*##*/
  376. TRSF_PTR t;
  377. int tm;
  378. char mt;
  379. {
  380.     double r;
  381.  
  382.     Write(&iobf_n[0], (r = t->p.x, (char *)&r), sizeof(double));
  383.     Write(&iobf_n[1], (r = t->p.y, (char *)&r), sizeof(double));
  384.     Write(&iobf_n[2], (r = t->p.z, (char *)&r), sizeof(double));
  385.     Write(&iobf_n[3], (r = t->n.x, (char *)&r), sizeof(double));
  386.     Write(&iobf_n[4], (r = t->n.y, (char *)&r), sizeof(double));
  387.     Write(&iobf_n[5], (r = t->n.z, (char *)&r), sizeof(double));
  388.     Write(&iobf_n[6], (r = t->o.x, (char *)&r), sizeof(double));
  389.     Write(&iobf_n[7], (r = t->o.y, (char *)&r), sizeof(double));
  390.     Write(&iobf_n[8], (r = t->o.z, (char *)&r), sizeof(double));
  391.     Write(&iobf_n[9], (r = t->a.x, (char *)&r), sizeof(double));
  392.     Write(&iobf_n[10], (r = t->a.y, (char *)&r), sizeof(double));
  393.     Write(&iobf_n[11], (r = t->a.z, (char *)&r), sizeof(double));
  394.     Write(&iobf_n[TB], (char *)&tm, sizeof(int));
  395.     Write(&iobf_n[CB], (char *)&mt, sizeof(char));
  396. }
  397.  
  398. static double rngp(a) /*##*/
  399. double a;
  400. {
  401.     while(a <= -pi_m)
  402.         a += pit2_m;
  403.     while(a > pi_m)
  404.         a -= pit2_m;
  405.     return(a);
  406. }
  407.  
  408.  
  409. /*
  410.  * output j6 in a buffered manner
  411.  */
  412.  
  413. static outjns(j, tm, mt) /*##*/
  414. JNTS_PTR j;
  415. {
  416.     double r;
  417.  
  418.     Write(&iobf_n[J1],
  419.         (r = ((opsw_n.angles) ? rngp(j->th1 + jmin_c.th1) : j->th1)
  420.         * rdtodg_m, (char *)&r), sizeof(double));
  421.     Write(&iobf_n[J2],
  422.         (r = ((opsw_n.angles) ? rngp(j->th2 + jmin_c.th2) : j->th2)
  423.         * rdtodg_m, (char *)&r), sizeof(double));
  424. #ifdef PUMA
  425.     Write(&iobf_n[J3],
  426.         (r = ((opsw_n.angles) ? rngp(j->th3 + jmin_c.th3) : j->th3)
  427.         * rdtodg_m, (char *)&r), sizeof(double));
  428. #endif
  429. #ifdef STAN
  430.     Write(&iobf_n[J3],
  431.         (r = ((opsw_n.angles) ? j->th3 + jmin_c.th3 : j->th3),
  432.         (char *)&r), sizeof(double));
  433. #endif
  434.     Write(&iobf_n[J4],
  435.         (r = ((opsw_n.angles) ? rngp(j->th4 + jmin_c.th4) : j->th4)
  436.         * rdtodg_m, (char *)&r), sizeof(double));
  437.     Write(&iobf_n[J5],
  438.         (r = ((opsw_n.angles) ? rngp(j->th5 + jmin_c.th5) : j->th5)
  439.         * rdtodg_m, (char *)&r), sizeof(double));
  440.     Write(&iobf_n[J6],
  441.         (r = ((opsw_n.angles) ? rngp(j->th6 + jmin_c.th6) : j->th6)
  442.         * rdtodg_m, (char *)&r), sizeof(double));
  443.     Write(&iobf_n[TB], (char *)&tm, sizeof(int));
  444.     Write(&iobf_n[CB], (char *)&mt, sizeof(char));
  445. }
  446.  
  447.  
  448. /*
  449.  * print t6 on @.out
  450.  */
  451.  
  452. static prtmat(p, t, ts, tm, mt, fpt) /*##*/
  453. PST_PTR p;
  454. TRSF_PTR t;
  455. int ts, tm;
  456. char mt;
  457. FILE *fpt;
  458. {
  459.     fprintf(fpt, "%8s ", p->name);
  460.     fprintf(fpt, "%c %4d %4d ", mt, tm, ts);
  461.     fprintf(fpt,
  462. "%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
  463.     t->p.x, t->p.y, t->p.z,
  464.     t->n.x, t->n.y, t->n.z,
  465.     t->o.x, t->o.y, t->o.z,
  466.     t->a.x, t->a.y, t->a.z);
  467. }
  468.  
  469.  
  470. /*
  471.  * print j6 on @.out
  472.  */
  473.  
  474. static prtjns(p, j, ts, tm, mt, cpyj, fpt) /*##*/
  475. PST_PTR p;
  476. JNTS_PTR j;
  477. int ts, tm;
  478. char mt;
  479. int cpyj;
  480. FILE *fpt;
  481. {
  482.     fprintf(fpt, "%8s ", p->name);
  483.     fprintf(fpt, "%c %4d %4d ", mt, tm, ts);
  484.     fprintf(fpt, "%7.2f %7.2f %7.2f %7.2f %7.2f %7.2f %s %o\n",
  485.     rdtodg_m * ((opsw_n.angles) ? rngp(j->th1 + jmin_c.th1) : j->th1),
  486.     rdtodg_m * ((opsw_n.angles) ? rngp(j->th2 + jmin_c.th2) : j->th2),
  487. #ifdef PUMA
  488.     rdtodg_m * ((opsw_n.angles) ? rngp(j->th3 + jmin_c.th3) : j->th3),
  489. #endif
  490. #ifdef STAN
  491.     ((opsw_n.angles) ? j->th3 + j->th3 : jmin_c.th3),
  492. #endif
  493.     rdtodg_m * ((opsw_n.angles) ? rngp(j->th4 + jmin_c.th4) : j->th4),
  494.     rdtodg_m * ((opsw_n.angles) ? rngp(j->th5 + jmin_c.th5) : j->th5),
  495.     rdtodg_m * ((opsw_n.angles) ? rngp(j->th6 + jmin_c.th6) : j->th6),
  496.     j->conf, cpyj);
  497. }
  498.  
  499. /*
  500.  * buffered writes, to avoid too many syscall
  501.  */
  502.  
  503. static Write(b, d, s) /*##*/
  504. BIO_PTR b;
  505. char *d;
  506. int s;
  507. {
  508.     if (b->ptr == NULL) {
  509.         b->ptr = b->buf;
  510.     }
  511.     while (s--) {
  512.         *(b->ptr)++ = *d++;
  513.         if (b->ptr - b->buf == BUFS) {
  514.             if (write(b->fd, b->buf, BUFS) != BUFS) {
  515.                 fprintf(stderr, "Write io error\n");
  516.                 exit(2);
  517.             }
  518.             b->ptr = b->buf;
  519.         }
  520.     }
  521. }
  522. #endif /* not REAL */
  523.