home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL094 < prev    next >
Encoding:
Text File  |  1987-03-02  |  5.7 KB  |  266 lines
  1. /*
  2.  * RTC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : rtc
  6.  *      File    : cvae.c
  7.  *      Remarks : Mapping functions encoder/angles/ranges cur/torques.
  8.  *                Torque stuff not implemented for the stanford arm.
  9.  *      Usage   : part of the library
  10.  */
  11.  
  12. /*LINTLIBRARY*/
  13.  
  14. #include "../h/which.h"
  15. #include "../h/umac.h"
  16. #ifdef PUMA
  17. #include "../h/pumadata.h"
  18. #include "../h/pumaload.h"
  19. #endif
  20. #ifdef STAN
  21. #include "../h/standata.h"
  22. #include "../h/stanload.h"
  23. #endif
  24.  
  25. #define J1      0
  26. #define J2      1
  27. #define J3      2
  28. #define J4      3
  29. #define J5      4
  30. #define J6      5
  31. #define PIT2    6.28318530717958650
  32. #define PI      3.14159265358979320
  33. #define DEGTORAD        0.01745329251994330
  34.  
  35.  
  36. /*
  37.  * take ranges, make encoder values
  38.  * return : 01 02 04 010 020 040 'ored' codes if joint within 5 dg limit
  39.  */
  40.  
  41. #define BOUND   5. * DEGTORAD
  42.  
  43. rngtoenc(e, r)  /*::*/
  44. register unsigned short *e;
  45. register double *r;
  46. {
  47.     register int code = 0;
  48.  
  49.     if (r[J1] < BOUND || r[J1] > JRNG1 - BOUND)
  50.         code |= 01;
  51.     if (r[J2] < BOUND || r[J2] > JRNG2 - BOUND)
  52.         code |= 02;
  53.     if (r[J3] < BOUND || r[J3] > JRNG3 - BOUND)
  54.         code |= 04;
  55.     if (r[J4] < BOUND || r[J4] > JRNG4 - BOUND)
  56.         code |= 010;
  57.     if (r[J5] < BOUND || r[J5] > JRNG5 - BOUND)
  58.         code |= 020;
  59.     if (r[J6] < BOUND || r[J6] > JRNG6 - BOUND)
  60.         code |= 040;
  61.     e[J1] = (int)(r[J1] * RATIO1) - OFFSET1;
  62.     e[J2] = (int)(r[J2] * RATIO2) - OFFSET2;
  63.     e[J3] = (int)(r[J3] * RATIO3) - OFFSET3;
  64.     e[J4] = (int)(r[J4] * RATIO4) - OFFSET4;
  65. #ifdef STAN
  66.     e[J5] = (int)(r[J5] * RATIO5) - OFFSET5;
  67.     e[J6] = (int)(r[J6] * RATIO6) - OFFSET6;
  68. #endif
  69. #ifdef PUMA
  70.     e[J5] = (int)(r[J5] * RATIO5 + r[J4] * RATI54) - OFFSET5;
  71.     e[J6] = (int)(r[J6] * RATIO6 + r[J5] * RATI65 + r[J4] * RATI64)
  72.          - OFFSET6;
  73. #endif
  74.     return(code);
  75. }
  76.  
  77. /*
  78.  * convert encoder values to ranges
  79.  */
  80.  
  81. enctorng(r, e) /*::*/
  82. register double *r;
  83. register unsigned short *e;
  84. {
  85.     r[J1] = (double)(e[J1] + OFFSET1) / RATIO1;
  86.     r[J2] = (double)(e[J2] + OFFSET2) / RATIO2;
  87.     r[J3] = (double)(e[J3] + OFFSET3) / RATIO3;
  88.     r[J4] = (double)(e[J4] + OFFSET4) / RATIO4;
  89. #ifdef STAN
  90.     r[J5] = (double)(e[J5] + OFFSET5) / RATIO5;
  91.     r[J6] = (double)(e[J6] + OFFSET6) / RATIO6;
  92. #endif
  93. #ifdef PUMA
  94.     r[J5] = ((double)(e[J5] + OFFSET5) - r[J4] * RATI54) / RATIO5;
  95.     r[J6] = ((double)(e[J6] + OFFSET6) - r[J5] * RATI65 - r[J4] * RATI64)
  96.          / RATIO6;
  97. #endif
  98. }
  99.  
  100.  
  101. static double rngpp(a) /*##*/
  102. double a;
  103. {
  104.     while(a <= -PI)
  105.         a += PIT2;
  106.     while(a > PI)
  107.         a -= PIT2;
  108.     return(a);
  109. }
  110.  
  111.  
  112.  
  113. static double rng0p(a) /*##*/
  114. double a;
  115. {
  116.     while(a < 0.)
  117.         a += PIT2;
  118.     while(a >= PIT2)
  119.         a -= PIT2;
  120.     return(a);
  121. }
  122.  
  123. /*
  124.  * convert ranges to angles
  125.  */
  126.  
  127. rngtoang(a ,r) /*::*/
  128. register double *a, *r;
  129. {
  130.     a[J1] = rngpp(r[J1] + JMIN1);
  131.     a[J2] = rngpp(r[J2] + JMIN2);
  132. #ifdef PUMA
  133.     a[J3] = rngpp(r[J3] + JMIN3);
  134. #endif
  135. #ifdef STAN
  136.     a[J3] = r[J3] + JMIN3;
  137. #endif
  138.     a[J4] = rngpp(r[J4] + JMIN4);
  139.     a[J5] = rngpp(r[J5] + JMIN5);
  140.     a[J6] = rngpp(r[J6] + JMIN6);
  141. }
  142.  
  143.  
  144. /*
  145.  * convert angles to ranges
  146.  */
  147.  
  148. angtorng(r, a) /*::*/
  149. register double *r, *a;
  150. {
  151.     r[J1] = rng0p(a[J1] - JMIN1);
  152.     r[J2] = rng0p(a[J2] - JMIN2);
  153. #ifdef PUMA
  154.     r[J3] = rng0p(a[J3] - JMIN3);
  155. #endif
  156. #ifdef STAN
  157.     r[J3] = a[J3] - JMIN3;
  158. #endif
  159.     r[J4] = rng0p(a[J4] - JMIN4);
  160.     r[J5] = rng0p(a[J5] - JMIN5);
  161.     r[J6] = rng0p(a[J6] - JMIN6);
  162. }
  163.  
  164.  
  165. /*
  166.  * convert angles to encoders
  167.  * return : 01 02 04 010 020 040 'ored' codes if joint within 5 dg limit
  168.  */
  169.  
  170. angtoenc(e, a) /*::*/
  171. register double *a;
  172. register unsigned short *e;
  173. {
  174.     double r[6];
  175.  
  176.     angtorng(r, a);
  177.     return(rngtoenc(e, r));
  178. }
  179.  
  180.  
  181. /*
  182.  * convert encoders to angles
  183.  */
  184.  
  185. enctoang(a, e) /*::*/
  186. register unsigned short *e;
  187. register double *a;
  188. {
  189.     enctorng(a, e);
  190.     rngtoang(a, a);
  191. }
  192.  
  193.  
  194. /*
  195.  * map torques to currents, add coulomb friction terms
  196.  */
  197.  
  198. tortodac(d, t, v, o) /*::*/
  199. register
  200. short *d;
  201. register
  202. double *t;
  203. register
  204. unsigned short *v, *o;
  205. {
  206.     register int vp;        /* velocity positive predicate  */
  207.  
  208.     vp = v[J1] < o[J1];
  209.     d[J1] = (v[J1] == o[J1]) ? (t[J1] * TDA1) :
  210.         (t[J1] * ((vp) ? TDPA1 : TDNA1)) + ((vp) ? TDPB1 : TDNB1);
  211.  
  212.     vp = v[J2] > o[J2];
  213.     d[J2] = (v[J2] == o[J2]) ? (t[J2] * TDA2) :
  214.         (t[J2] * ((vp) ? TDPA2 : TDNA2)) + ((vp) ? TDPB2 : TDNB2);
  215.  
  216.     vp = v[J3] < o[J3];
  217.     d[J3] = (v[J3] == o[J3]) ? (t[J3] * TDA3) :
  218.         (t[J3] * ((vp) ? TDPA3 : TDNA3)) + ((vp) ? TDPB3 : TDNB3);
  219.  
  220.     vp = v[J4] > o[J4];
  221.     d[J4] = (v[J4] == o[J4]) ? (t[J4] * TDA4) :
  222.         (t[J4] * ((vp) ? TDPA4 : TDNA4)) + ((vp) ? TDPB4 : TDNB4);
  223.  
  224.     vp = v[J5] > o[J5];
  225.     d[J5] = (v[J5] == o[J5]) ? (t[J5] * TDA5) :
  226.         (t[J5] * ((vp) ? TDPA5 : TDNA5)) + ((vp) ? TDPB5 : TDNB5);
  227.  
  228.     vp = v[J6] > o[J6];
  229.     d[J6] = (v[J6] == o[J6]) ? (t[J6] * TDA6) :
  230.         (t[J6] * ((vp) ? TDPA6 : TDNA6)) + ((vp) ? TDPB6 : TDNB6);
  231. }
  232.  
  233.  
  234. /*
  235.  * map currents to torques, remove coulomb friction terms
  236.  */
  237.  
  238. adctotor(t, a, v, o) /*::*/
  239. register
  240. double *t;
  241. register
  242. short *a;
  243. register
  244. unsigned short *v, *o;
  245. {
  246.     register int vp;        /* velocity positive predicate  */
  247.  
  248.     vp = v[J1] < o[J1];
  249.     t[J1] = (a[J1] * ((vp) ? ATPA1 : ATNA1)) + ((vp) ? ATPB1 : ATNB1);
  250.  
  251.     vp = v[J2] > o[J2];
  252.     t[J2] = (a[J2] * ((vp) ? ATPA2 : ATNA2)) + ((vp) ? ATPB2 : ATNB2);
  253.  
  254.     vp = v[J3] < o[J3];
  255.     t[J3] = (a[J3] * ((vp) ? ATPA3 : ATNA3)) + ((vp) ? ATPB3 : ATNB3);
  256.  
  257.     vp = v[J4] > o[J4];
  258.     t[J4] = (a[J4] * ((vp) ? ATPA4 : ATNA4)) + ((vp) ? ATPB4 : ATNB4);
  259.  
  260.     vp = v[J5] > o[J5];
  261.     t[J5] = (a[J5] * ((vp) ? ATPA5 : ATNA5)) + ((vp) ? ATPB5 : ATNB5);
  262.  
  263.     vp = v[J6] > o[J6];
  264.     t[J6] = (a[J6] * ((vp) ? ATPA6 : ATNA6)) + ((vp) ? ATPB6 : ATNB6);
  265. }
  266.