home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT03.ZIP / RCCL166 < prev    next >
Encoding:
Text File  |  1987-03-02  |  10.4 KB  |  319 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  * RTC          2.0                     School of Electrical Engineering
  4.  * MAC          2.0                     Purdue University
  5.  *      Dir     : h
  6.  *      File    : pumac.c
  7.  *      Remarks : generates the include file standata.h containing all the
  8.  *                precalculated constants of the puma Stanford arm.
  9.  *                Due to hardware problems, most of the constants have
  10.  *                to be revised.
  11.  *      Usage   : cc stanc.c -lm ; a.out
  12.  */
  13.  
  14. /*************************************************************************
  15.  notations : { E } : E expressed in degrees converted in radians
  16.          [ E ] : E of type double converted in type int
  17.          < E > : E ....... int    ................. short
  18.          | E | : mod 2*pi ; E + n*2*pi , n so that   0 <= | E | < 2*pi
  19.          constants in upercase letters, variables in lower.
  20.          (d) type double, (i) type int, (s) type short
  21.  
  22.       min is chosen as the furthest position clockwise
  23.       angles increase in counter-clockwise motion
  24.       rng is the angle between min and max positions
  25.       relative coordinates measure angles in respect with the min pos
  26.  
  27.  let :
  28.        DMIN  (d) the minimum position in deg.  absolute coordinates
  29.        JMIN  (d) .................... in rad.  ....................
  30.        DRNG  (d) the range in deg.
  31.        JRNG  (d) ............ rad.
  32.        DCAL  (d) the calibration position in deg. absolute coordinates
  33.        ACAL  (d) ........................ in rad  ....................
  34.        JCAL  (d) ........................ in rad. relative coordinates
  35.        ECCL  (i) the counters content at the calibration position
  36.        RATIO (d) the ratio  encoder pulses / physical unitd (mm,rad)
  37.        sol   (d) be the raw angle given by the solution equation
  38.        j     (d) a joint value expressed in relative coordinates
  39.        e     (s) the final encoder value
  40.        DMXV  (d) the maximum joint velocity in deg. / sec.
  41.        JMXV  (d) the ......................... rd. / sec.
  42.  
  43.   this program generates the following constantes :
  44.  
  45.      JMIN = { DMIN }
  46.      JRNG = { DRNG }
  47.      RATIO = RATIO
  48.      OFFSET = [ | { DCAL - DMIN } | * RATIO ] - ECCL
  49.      JCAL = | { DCAL - DMIN } |
  50.      ACAL = { DCAL }
  51.      JMXV = { DMXV }
  52.  
  53.   to be used by :
  54.      calibrate :
  55.     sets  j = JCAL
  56.     computes the REST transform from the joints values
  57.  
  58.      tr_to_jns :
  59.     computes   j = | sol - JMIN |   and checks if  j < JRNG
  60.  
  61.      putenco  :
  62.     e = < [ j * RATIO ] - OFFSET >
  63.  
  64.      setpoint :
  65.     to monitor joint velocities
  66.  
  67. ********************************************************************/
  68.  
  69. #include <stdio.h>
  70.  
  71. #define PIB2      1.57079632679489660
  72. #define PI        3.14159265358979320
  73. #define PIT2      6.28318530717958650
  74. #define RADTODEG 57.29577951308232100
  75. #define DEGTORAD  0.01745329251994330
  76. #define YES 1
  77. #define NO  0
  78.  
  79. #define D2 174.00   /* mm */
  80.  
  81. #define DMIN1 220.0
  82. #define DRNG1 280.0
  83.  
  84. #define DMIN2   0.0
  85. #define DRNG2 180.0
  86.  
  87. #define DMIN3 195.0
  88. #define DRNG3 690.0
  89.  
  90. #define DMIN4 180.0
  91. #define DRNG4 360.0
  92.  
  93. #define DMIN5 260.0
  94. #define DRNG5 200.0
  95.  
  96. #define DMIN6 180.0
  97. #define DRNG6 360.0
  98.  
  99. #define RATIO1 -12732.39545  /* pls/rad */
  100. #define RATIO2  15915.49431  /* pls/rad */
  101. #define RATIO3  35.64625147  /* pls/mm */
  102. #define RATIO4 -4583.662360  /* pls/rad */
  103. #define RATIO5  4583.662360  /* pls/rad */
  104. #define RATIO6 -5570.423007  /* pls/rad */
  105.  
  106.  
  107. #define ECCL1 32768
  108. #define ECCL2 32768
  109. #define ECCL3 26468
  110. #define ECCL4 32768
  111. #define ECCL5 25568
  112. #define ECCL6 32768
  113.  
  114. #define ESQR1   551             /* how much to bring the arm in */
  115. #define ESQR2   263             /* a square configuration from the */
  116. #define ESQR3   581             /* zero index configuration */
  117. #define ESQR4   228
  118. #define ESQR5   -16
  119. #define ESQR6   -13
  120.  
  121.  
  122. #define DCAL1   0.0
  123. #define DCAL2  90.0
  124. #define DCAL3 385.0
  125. #define DCAL4   0.0
  126. #define DCAL5  90.0
  127. #define DCAL6   0.0
  128.  
  129.  
  130. #define DMXV1   600.
  131. #define DMXV2   600.
  132. #define DMXV3   500.    /* mm/sec */
  133. #define DMXV4   600.
  134. #define DMXV5   600.
  135. #define DMXV6   600.
  136.  
  137. #define MXDC1   300
  138. #define MXDC2   300
  139. #define MXDC3   200
  140. #define MXDC4   200
  141. #define MXDC5   200
  142. #define MXDC6   200
  143.  
  144.  
  145. #define MXOC1   250
  146. #define MXOC2   250
  147. #define MXOC3   200
  148. #define MXOC4   200
  149. #define MXOC5   200
  150. #define MXOC6   200
  151.  
  152. double range(a)
  153. double a;
  154. {
  155.     while(a < 0.)
  156.         a += PIT2;
  157.     while(a >= PIT2)
  158.         a -= PIT2;
  159.     return(a);
  160. }
  161.  
  162.  
  163. #define ABS(a) ((a < 0) ? (-(a)) : (a))
  164. #define ORDER(a, b) {int t; if (a > b) {t = b; b = a; a = t;}}
  165.  
  166. /*
  167.  * create #define constants for stanford arm
  168.  */
  169.  
  170. main()
  171. {
  172.     int o1, o2, o3, o4, o5, o6;
  173.     int mx1, mx2, mx3, mx4, mx5, mx6;
  174.     int mn1, mn2, mn3, mn4, mn5, mn6;
  175.     FILE *fph;
  176.  
  177.     fph = fdopen(creat("standata.h", 0644), "w");
  178.  
  179.     fprintf(fph, "#define D2      %20.12f\n", D2);
  180.     fprintf(fph, "#define D22     %20.12f\n", D2 * D2);
  181.     fprintf(fph, "#define JMIN1   %20.12f\n", DEGTORAD * DMIN1);
  182.     fprintf(fph, "#define JRNG1   %20.12f\n", DEGTORAD * DRNG1);
  183.     fprintf(fph, "#define ACAL1   %20.12f\n", DEGTORAD * DCAL1);
  184.     fprintf(fph, "#define JMIN2   %20.12f\n", DEGTORAD * DMIN2);
  185.     fprintf(fph, "#define JRNG2   %20.12f\n", DEGTORAD * DRNG2);
  186.     fprintf(fph, "#define ACAL2   %20.12f\n", DEGTORAD * DCAL2);
  187.     fprintf(fph, "#define JMIN3   %20.12f\n",            DMIN3);
  188.     fprintf(fph, "#define JRNG3   %20.12f\n",            DRNG3);
  189.     fprintf(fph, "#define ACAL3   %20.12f\n",            DCAL3);
  190.     fprintf(fph, "#define JMIN4   %20.12f\n", DEGTORAD * DMIN4);
  191.     fprintf(fph, "#define JRNG4   %20.12f\n", DEGTORAD * DRNG4);
  192.     fprintf(fph, "#define ACAL4   %20.12f\n", DEGTORAD * DCAL4);
  193.     fprintf(fph, "#define JMIN5   %20.12f\n", DEGTORAD * DMIN5);
  194.     fprintf(fph, "#define JRNG5   %20.12f\n", DEGTORAD * DRNG5);
  195.     fprintf(fph, "#define ACAL5   %20.12f\n", DEGTORAD * DCAL5);
  196.     fprintf(fph, "#define JMIN6   %20.12f\n", DEGTORAD * DMIN6);
  197.     fprintf(fph, "#define JRNG6   %20.12f\n", DEGTORAD * DRNG6);
  198.     fprintf(fph, "#define ACAL6   %20.12f\n", DEGTORAD * DCAL6);
  199.  
  200.     fprintf(fph, "#define JCAL1   %20.12f\n",
  201.     range(DEGTORAD * (DCAL1 - DMIN1)));
  202.     fprintf(fph, "#define JCAL2   %20.12f\n",
  203.     range(DEGTORAD * (DCAL2 - DMIN2)));
  204.     fprintf(fph, "#define JCAL3   %20.12f\n",
  205.      /* LIN JOINT */  DCAL3 - DMIN3);
  206.     fprintf(fph, "#define JCAL4   %20.12f\n",
  207.     range(DEGTORAD * (DCAL4 - DMIN4)));
  208.     fprintf(fph, "#define JCAL5   %20.12f\n",
  209.     range(DEGTORAD * (DCAL5 - DMIN5)));
  210.     fprintf(fph, "#define JCAL6   %20.12f\n",
  211.     range(DEGTORAD * (DCAL6 - DMIN6)));
  212.  
  213.     fprintf(fph, "#define RATIO1  %20.12f\n", RATIO1);
  214.     fprintf(fph, "#define RATIO2  %20.12f\n", RATIO2);
  215.     fprintf(fph, "#define RATIO3  %20.12f\n", RATIO3);
  216.     fprintf(fph, "#define RATIO4  %20.12f\n", RATIO4);
  217.     fprintf(fph, "#define RATIO5  %20.12f\n", RATIO5);
  218.     fprintf(fph, "#define RATIO6  %20.12f\n", RATIO6);
  219.  
  220.     fprintf(fph, "#define OFFSET1   (%6d)\n", o1 =
  221.     (int)(range((DCAL1 - DMIN1) * DEGTORAD) * RATIO1) - ECCL1);
  222.  
  223.     fprintf(fph, "#define OFFSET2   (%6d)\n", o2 =
  224.     (int)(range((DCAL2 - DMIN2) * DEGTORAD) * RATIO2) - ECCL2);
  225.  
  226.     fprintf(fph, "#define OFFSET3   (%6d)\n", o3 =
  227.     (int)((DCAL3 - DMIN3) * RATIO3) - ECCL3);
  228.  
  229.     fprintf(fph, "#define OFFSET4   (%6d)\n", o4 =
  230.     (int)(range((DCAL4 - DMIN4) * DEGTORAD) * RATIO4) - ECCL4);
  231.  
  232.     fprintf(fph, "#define OFFSET5   (%6d)\n", o5 =
  233.     (int)(range((DCAL5 - DMIN5) * DEGTORAD) * RATIO5) - ECCL5);
  234.  
  235.     fprintf(fph, "#define OFFSET6   (%6d)\n", o6 =
  236.     (int)(range((DCAL6 - DMIN6) * DEGTORAD) * RATIO6) - ECCL6);
  237.  
  238.     fprintf(fph, "#define JMXV1 %f\n",  DEGTORAD * DMXV1);
  239.     fprintf(fph, "#define JMXV2 %f\n",  DEGTORAD * DMXV2);
  240.     fprintf(fph, "#define JMXV3 %f\n",             DMXV3);
  241.     fprintf(fph, "#define JMXV4 %f\n",  DEGTORAD * DMXV4);
  242.     fprintf(fph, "#define JMXV5 %f\n",  DEGTORAD * DMXV5);
  243.     fprintf(fph, "#define JMXV6 %f\n",  DEGTORAD * DMXV6);
  244.  
  245.     fprintf(fph, "#define EMXV1 0%o\n",
  246.     (int)(DEGTORAD * DMXV1 * ABS(RATIO1) / 1000.));
  247.     fprintf(fph, "#define EMXV2 0%o\n",
  248.     (int)(DEGTORAD * DMXV2 * ABS(RATIO2) / 1000.));
  249.     fprintf(fph, "#define EMXV3 0%o\n",
  250.     (int)(           DMXV3 * ABS(RATIO3) / 1000.));
  251.     fprintf(fph, "#define EMXV4 0%o\n",
  252.     (int)(DEGTORAD * DMXV4 * ABS(RATIO4) / 1000.));
  253.     fprintf(fph, "#define EMXV5 0%o\n",
  254.     (int)(DEGTORAD * DMXV5 * ABS(RATIO5) / 1000.));
  255.     fprintf(fph, "#define EMXV6 0%o\n",
  256.     (int)(DEGTORAD * DMXV6 * ABS(RATIO6) / 1000.));
  257.  
  258.     fprintf(fph, "#define ECCL1 0%o\n", ECCL1);
  259.     fprintf(fph, "#define ECCL2 0%o\n", ECCL2);
  260.     fprintf(fph, "#define ECCL3 0%o\n", ECCL3);
  261.     fprintf(fph, "#define ECCL4 0%o\n", ECCL4);
  262.     fprintf(fph, "#define ECCL5 0%o\n", ECCL5);
  263.     fprintf(fph, "#define ECCL6 0%o\n", ECCL6);
  264.  
  265.     fprintf(fph, "#define XCCL1 0%o\n", ECCL1 + ESQR1);
  266.     fprintf(fph, "#define XCCL2 0%o\n", ECCL2 + ESQR2);
  267.     fprintf(fph, "#define XCCL3 0%o\n", ECCL3 + ESQR3);
  268.     fprintf(fph, "#define XCCL4 0%o\n", ECCL4 + ESQR4);
  269.     fprintf(fph, "#define XCCL5 0%o\n", ECCL5 + ESQR5);
  270.     fprintf(fph, "#define XCCL6 0%o\n", ECCL6 + ESQR6);
  271.  
  272.     mn1 = -o1;
  273.     mx1 = (int)(DEGTORAD * DRNG1 * RATIO1) - o1;
  274.     ORDER(mn1, mx1);
  275.     mn2 = -o2;
  276.     mx2 = (int)(DEGTORAD * DRNG2 * RATIO2) - o2;
  277.     ORDER(mn2, mx2);
  278.     mn3 = -o3;
  279.     mx3 = (int)(           DRNG3 * RATIO3) - o3;
  280.     ORDER(mn3, mx3);
  281.     mn4 = -o4;
  282.     mx4 = (int)(DEGTORAD * DRNG4 * RATIO4) - o4;
  283.     ORDER(mn4, mx4);
  284.     mn5 = -o5;
  285.     mx5 = (int)(DEGTORAD * DRNG5 * RATIO5) - o5;
  286.     ORDER(mn5, mx5);
  287.     mn6 = -o6;
  288.     mx6 = (int)(DEGTORAD * DRNG6 * RATIO6) - o6;
  289.     ORDER(mn6, mx6);
  290.  
  291.     fprintf(fph, "#define ECMN1 0%o\n", mn1);
  292.     fprintf(fph, "#define ECMN2 0%o\n", mn2);
  293.     fprintf(fph, "#define ECMN3 0%o\n", mn3);
  294.     fprintf(fph, "#define ECMN4 0%o\n", mn4);
  295.     fprintf(fph, "#define ECMN5 0%o\n", mn5);
  296.     fprintf(fph, "#define ECMN6 0%o\n", mn6);
  297.  
  298.     fprintf(fph, "#define ECMX1 0%o\n", mx1);
  299.     fprintf(fph, "#define ECMX2 0%o\n", mx2);
  300.     fprintf(fph, "#define ECMX3 0%o\n", mx3);
  301.     fprintf(fph, "#define ECMX4 0%o\n", mx4);
  302.     fprintf(fph, "#define ECMX5 0%o\n", mx5);
  303.     fprintf(fph, "#define ECMX6 0%o\n", mx6);
  304.  
  305.     fprintf(fph, "#define MXDC1 0%o\n", MXDC1);
  306.     fprintf(fph, "#define MXDC2 0%o\n", MXDC2);
  307.     fprintf(fph, "#define MXDC3 0%o\n", MXDC3);
  308.     fprintf(fph, "#define MXDC4 0%o\n", MXDC4);
  309.     fprintf(fph, "#define MXDC5 0%o\n", MXDC5);
  310.     fprintf(fph, "#define MXDC6 0%o\n", MXDC6);
  311.  
  312.     fprintf(fph, "#define MXOC1 0%o\n", MXOC1);
  313.     fprintf(fph, "#define MXOC2 0%o\n", MXOC2);
  314.     fprintf(fph, "#define MXOC3 0%o\n", MXOC3);
  315.     fprintf(fph, "#define MXOC4 0%o\n", MXOC4);
  316.     fprintf(fph, "#define MXOC5 0%o\n", MXOC5);
  317.     fprintf(fph, "#define MXOC6 0%o\n", MXOC6);
  318. }
  319.