home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT03.ZIP / RCCL168 < prev    next >
Encoding:
Text File  |  1987-03-02  |  12.5 KB  |  362 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 pumadata.h containing all the
  8.  *                precalculated constants of the puma 600.
  9.  *      Usage   : cc pumac.c -lm ; a.out
  10.  */
  11.  
  12. /*************************************************************************
  13.  notations : { E } : E expressed in degrees converted to radians
  14.          [ E ] : E of type double converted to type int
  15.          < E > : E ....... int    ................. short
  16.          | E | : mod 2*pi ; E + n*2*pi , n so that   0 <= | E | < 2*pi
  17.          constants in upercase letters, variables in lower.
  18.          (d) type double, (i) type int, (s) type short
  19.  
  20.       min is chosen as the furthest position clockwise
  21.       angles increase in counter-clockwise motion
  22.       rng is the angle between min and max positions
  23.       relative coordinates measure angles in respect with the min pos
  24.  
  25.  let :
  26.        DMIN  (d) the minimum position in deg.  absolute coordinates
  27.        JMIN  (d) .................... in rad.  ....................
  28.        DRNG  (d) the range in deg.
  29.        JRNG  (d) ............ rad.
  30.        DCAL  (d) the calibration position in deg. absolute coordinates
  31.        ACAL  (d) ........................ in rad  ....................
  32.        JCAL  (d) ........................ in rad. relative coordinates
  33.        ECCL  (i) the counters content at the calibration position
  34.        RATIO (d) the ratio  encoder pulses / physical unitd (mm,rad)
  35.        sol   (d) be the raw angle given by the solution equation
  36.        j     (d) a joint value expressed in relative coordinates
  37.        e     (s) the final encoder value
  38.        DMXV  (d) the maximum joint velocity in deg. / sec.
  39.        JMXV  (d) the ......................... rd. / sec.
  40.  
  41.   this program generates the following constantes :
  42.  
  43.      JMIN = { DMIN }
  44.      JRNG = { DRNG }
  45.      RATIO = RATIO
  46.      OFFSET = [ | { DCAL - DMIN } | * RATIO ] - ECCL
  47.      JCAL = | { DCAL - DMIN } |
  48.      ACAL = { DCAL }
  49.      JMXV = { DMXV }
  50.      EMXV = [ JMXV * RATIO / 1000]      (max enc inc. in 1 ms)
  51.      ECMN = - OFFSET                    (min enc value)
  52.      ECMX = { DRNG } * RATIO - OFFSET   (max enc value)
  53.      MXDC = MXDC                        (max desired current in dac readings)
  54.      MXOC = MXOC                        (max observed current in adc readings)
  55.  
  56.   to be used by :
  57.      calibrate :
  58.     sets  j = JCAL
  59.     computes the REST transform from the joints values
  60.  
  61.      tr_to_jns :
  62.     computes   j = | sol - JMIN |   and checks if  j < JRNG
  63.  
  64.      putenco  :
  65.     e = < [ j * RATIO ] - OFFSET >
  66.     e5 = < [ j5 * RATIO5 + j4 * RATI54 ] - OFFSET5 >
  67.     e6 = < [ j6 * RATIO6 + j5 * RATI65 + j4 * RATI64 ] - OFFSET6 >
  68.  
  69.      setpoint :
  70.     to monitor joint velocities
  71.  
  72. ********************************************************************/
  73.  
  74. #include <stdio.h>
  75.  
  76. #define PIB2      1.57079632679489660
  77. #define PI        3.14159265358979320
  78. #define PIT2      6.28318530717958650
  79. #define RADTODEG 57.29577951308232100
  80. #define DEGTORAD  0.01745329251994330
  81. #define YES 1
  82. #define NO  0
  83.  
  84. /* dimension in millimeters */
  85.  
  86. #define A2 431.80
  87. #define A3 20.320
  88. #define D3 128.778
  89. #define D4 433.070
  90.  
  91. #define DMIN1 200.0
  92. #define DRNG1 320.0
  93.  
  94. #define DMIN2 137.0
  95. #define DRNG2 266.0
  96.  
  97. #define DMIN3 308.0
  98. #define DRNG3 284.0
  99.  
  100. #define DMIN4 250.0
  101. #define DRNG4 290.0
  102.  
  103. #define DMIN5 260.0
  104. #define DRNG5 200.0
  105.  
  106. #define DMIN6 180.0   /* was 210.0 in full count */
  107. #define DRNG6 360.0   /*     240.0               */
  108.  
  109.  
  110. #define RATIO1 -9964.8501  /* pls/rad */
  111. #define RATIO2 13727.4322  /* pls/rad */
  112. #define RATIO3 -8547.6231  /* pls/rad */
  113. #define RATIO4 12101.5626  /* pls/rad */
  114. #define RATIO5 11446.9010  /* pls/rad */
  115. #define RATIO6 06102.4752  /* pls/rad ,in full count was 12204.9560 */
  116. #define RATI54 (-168.2565) /* pls/rad */
  117. #define RATI64   (78.9028) /* pls/rad ,in full count was (157.8056) */
  118. #define RATI65 (1033.4033) /* pls/red, in full count was (2066.8067)*/
  119.  
  120.  
  121. #define ECCL1 32000 /*32768*/
  122. #define ECCL2 32000 /*32768*/
  123. #define ECCL3 19000 /*19341*/
  124. #define ECCL4 26000 /*32768*/
  125. #define ECCL5 40000 /*50749*/
  126. #define ECCL6 40000 /*32768*/
  127.  
  128. #define ESQR1  283              /* how much to bring the arm in */
  129. #define ESQR2  600      /* 704     a square configuration from the */
  130. #define ESQR3  449              /* zero index configuration */
  131. #define ESQR4  480
  132. #define ESQR5  146      /* 147 */
  133. #define ESQR6  185
  134.  
  135. #define DCAL1   0.0
  136. #define DCAL2 270.0
  137. #define DCAL3 180.0
  138. #define DCAL4   0.0
  139. #define DCAL5  90.0
  140. #define DCAL6   0.0
  141.  
  142.  
  143. #define DMXV1   660.
  144. #define DMXV2   660.
  145. #define DMXV3   660.
  146. #define DMXV4   660.
  147. #define DMXV5   660.
  148. #define DMXV6   660.
  149.  
  150.  
  151. #define MXDC1   1000    /* 500 */
  152. #define MXDC2   1000    /* 400 */
  153. #define MXDC3   1000    /* 300 */
  154. #define MXDC4    500    /* 200 */
  155. #define MXDC5    500    /* 200 */
  156. #define MXDC6    500    /* 200 */
  157.  
  158.  
  159. #define MXOC1   500     /* 350 */
  160. #define MXOC2   500     /* 350 */
  161. #define MXOC3   500     /* 350 */
  162. #define MXOC4   300
  163. #define MXOC5   300
  164. #define MXOC6   300
  165.  
  166.  
  167. #define CV      7.05            /* oz in -> N mm */
  168.  
  169. #define C21     ( 5500.00  )    /* this is in oz in */
  170. #define C31     (-30.0     )
  171. #define C32     ( 1100.0   )
  172. #define C50     (-70.0     )
  173.  
  174. double range(a)
  175. double a;
  176. {
  177.     while(a < 0.)
  178.         a += PIT2;
  179.     while(a >= PIT2)
  180.         a -= PIT2;
  181.     return(a);
  182. }
  183.  
  184. #define ABS(a) ((a < 0) ? (-(a)) : (a))
  185. #define ORDER(a, b) {int t; if (a > b) {t = b; b = a; a = t;}}
  186.  
  187. /*
  188.  * create #define constants for puma 600 arm
  189.  */
  190.  
  191. main()
  192. {
  193.     int o1, o2, o3, o4, o5, o6;
  194.     int mx1, mx2, mx3, mx4, mx5, mx6;
  195.     int mn1, mn2, mn3, mn4, mn5, mn6;
  196.     FILE *fph;
  197.  
  198.     fph = fdopen(creat("pumadata.h",  0644),"w");
  199.  
  200.     fprintf(fph, "#define A2      %20.12f\n",  A2);
  201.     fprintf(fph, "#define A3      %20.12f\n",  A3);
  202.     fprintf(fph, "#define D3      %20.12f\n",  D3);
  203.     fprintf(fph, "#define D4      %20.12f\n",  D4);
  204.  
  205.     fprintf(fph, "#define D32     %20.12f\n",  D3 * D3);
  206.     fprintf(fph, "#define E432    %20.12f\n",  D4 * D4 + A3*A3 + A2*A2);
  207.     fprintf(fph, "#define AA3D4   %20.12f\n",  atan2(A3,  -D4));
  208.     fprintf(fph, "#define E4AA4AD %20.12f\n",  4. * A2 * A2 * A3 * A3 +
  209.         4. * A2 * A2 * D4 * D4);
  210.     fprintf(fph, "#define JMIN1   %20.12f\n",  DEGTORAD * DMIN1);
  211.     fprintf(fph, "#define JRNG1   %20.12f\n",  DEGTORAD * DRNG1);
  212.     fprintf(fph, "#define ACAL1   %20.12f\n",  DEGTORAD * DCAL1);
  213.     fprintf(fph, "#define JMIN2   %20.12f\n",  DEGTORAD * DMIN2);
  214.     fprintf(fph, "#define JRNG2   %20.12f\n",  DEGTORAD * DRNG2);
  215.     fprintf(fph, "#define ACAL2   %20.12f\n",  DEGTORAD * DCAL2);
  216.     fprintf(fph, "#define JMIN3   %20.12f\n",  DEGTORAD * DMIN3);
  217.     fprintf(fph, "#define JRNG3   %20.12f\n",  DEGTORAD * DRNG3);
  218.     fprintf(fph, "#define ACAL3   %20.12f\n",  DEGTORAD * DCAL3);
  219.     fprintf(fph, "#define JMIN4   %20.12f\n",  DEGTORAD * DMIN4);
  220.     fprintf(fph, "#define JRNG4   %20.12f\n",  DEGTORAD * DRNG4);
  221.     fprintf(fph, "#define ACAL4   %20.12f\n",  DEGTORAD * DCAL4);
  222.     fprintf(fph, "#define JMIN5   %20.12f\n",  DEGTORAD * DMIN5);
  223.     fprintf(fph, "#define JRNG5   %20.12f\n",  DEGTORAD * DRNG5);
  224.     fprintf(fph, "#define ACAL5   %20.12f\n",  DEGTORAD * DCAL5);
  225.     fprintf(fph, "#define JMIN6   %20.12f\n",  DEGTORAD * DMIN6);
  226.     fprintf(fph, "#define JRNG6   %20.12f\n",  DEGTORAD * DRNG6);
  227.     fprintf(fph, "#define ACAL6   %20.12f\n",  DEGTORAD * DCAL6);
  228.  
  229.     fprintf(fph, "#define JCAL1   %20.12f\n",
  230.     range(DEGTORAD * (DCAL1 - DMIN1)));
  231.     fprintf(fph, "#define JCAL2   %20.12f\n",
  232.     range(DEGTORAD * (DCAL2 - DMIN2)));
  233.     fprintf(fph, "#define JCAL3   %20.12f\n",
  234.     range(DEGTORAD * (DCAL3 - DMIN3)));
  235.     fprintf(fph, "#define JCAL4   %20.12f\n",
  236.     range(DEGTORAD * (DCAL4 - DMIN4)));
  237.     fprintf(fph, "#define JCAL5   %20.12f\n",
  238.     range(DEGTORAD * (DCAL5 - DMIN5)));
  239.     fprintf(fph, "#define JCAL6   %20.12f\n",
  240.     range(DEGTORAD * (DCAL6 - DMIN6)));
  241.  
  242.     fprintf(fph, "#define RATIO1  %20.12f\n",  RATIO1);
  243.     fprintf(fph, "#define RATIO2  %20.12f\n",  RATIO2);
  244.     fprintf(fph, "#define RATIO3  %20.12f\n",  RATIO3);
  245.     fprintf(fph, "#define RATIO4  %20.12f\n",  RATIO4);
  246.     fprintf(fph, "#define RATIO5  %20.12f\n",  RATIO5);
  247.     fprintf(fph, "#define RATIO6  %20.12f\n",  RATIO6);
  248.     fprintf(fph, "#define RATI54  %20.12f\n",  RATI54);
  249.     fprintf(fph, "#define RATI65  %20.12f\n",  RATI65);
  250.     fprintf(fph, "#define RATI64  %20.12f\n",  RATI64);
  251.  
  252.     fprintf(fph, "#define OFFSET1   (%6d)\n", o1 =
  253.     (int)(range((DCAL1 - DMIN1) * DEGTORAD) * RATIO1) - ECCL1);
  254.  
  255.     fprintf(fph, "#define OFFSET2   (%6d)\n", o2 =
  256.     (int)(range((DCAL2 - DMIN2) * DEGTORAD) * RATIO2) - ECCL2);
  257.  
  258.     fprintf(fph, "#define OFFSET3   (%6d)\n", o3 =
  259.     (int)(range((DCAL3 - DMIN3) * DEGTORAD) * RATIO3) - ECCL3);
  260.  
  261.     fprintf(fph, "#define OFFSET4   (%6d)\n", o4 =
  262.     (int)(range((DCAL4 - DMIN4) * DEGTORAD) * RATIO4) - ECCL4);
  263.  
  264.     fprintf(fph, "#define OFFSET5   (%6d)\n", o5 =
  265.     (int)(  range((DCAL5 - DMIN5) * DEGTORAD) * RATIO5
  266.           + range((DCAL4 - DMIN4) * DEGTORAD) * RATI54) - ECCL5);
  267.  
  268.     fprintf(fph, "#define OFFSET6   (%6d)\n", o6 =
  269.     (int)(  range((DCAL6 - DMIN6) * DEGTORAD) * RATIO6
  270.           + range((DCAL5 - DMIN5) * DEGTORAD) * RATI65
  271.           + range((DCAL4 - DMIN4) * DEGTORAD) * RATI64) - ECCL6);
  272.  
  273.     fprintf(fph, "#define JMXV1 %f\n",  DEGTORAD * DMXV1);
  274.     fprintf(fph, "#define JMXV2 %f\n",  DEGTORAD * DMXV2);
  275.     fprintf(fph, "#define JMXV3 %f\n",  DEGTORAD * DMXV3);
  276.     fprintf(fph, "#define JMXV4 %f\n",  DEGTORAD * DMXV4);
  277.     fprintf(fph, "#define JMXV5 %f\n",  DEGTORAD * DMXV5);
  278.     fprintf(fph, "#define JMXV6 %f\n",  DEGTORAD * DMXV6);
  279.  
  280.     fprintf(fph, "#define EMXV1 0%o\n",
  281.     (int)(DEGTORAD * DMXV1 * ABS(RATIO1) / 1000.));
  282.     fprintf(fph, "#define EMXV2 0%o\n",
  283.     (int)(DEGTORAD * DMXV2 * ABS(RATIO2) / 1000.));
  284.     fprintf(fph, "#define EMXV3 0%o\n",
  285.     (int)(DEGTORAD * DMXV3 * ABS(RATIO3) / 1000.));
  286.     fprintf(fph, "#define EMXV4 0%o\n",
  287.     (int)(DEGTORAD * DMXV4 * ABS(RATIO4) / 1000.));
  288.     fprintf(fph, "#define EMXV5 0%o\n",
  289.     (int)(DEGTORAD * DMXV5 * ABS(RATIO5) / 1000.));
  290.     fprintf(fph, "#define EMXV6 0%o\n",
  291.     (int)(DEGTORAD * DMXV6 * ABS(RATIO6) / 1000.));
  292.  
  293.     fprintf(fph, "#define ECCL1 0%o\n", ECCL1);
  294.     fprintf(fph, "#define ECCL2 0%o\n", ECCL2);
  295.     fprintf(fph, "#define ECCL3 0%o\n", ECCL3);
  296.     fprintf(fph, "#define ECCL4 0%o\n", ECCL4);
  297.     fprintf(fph, "#define ECCL5 0%o\n", ECCL5);
  298.     fprintf(fph, "#define ECCL6 0%o\n", ECCL6);
  299.  
  300.     fprintf(fph, "#define XCCL1 0%o\n", ECCL1 + ESQR1);
  301.     fprintf(fph, "#define XCCL2 0%o\n", ECCL2 + ESQR2);
  302.     fprintf(fph, "#define XCCL3 0%o\n", ECCL3 + ESQR3);
  303.     fprintf(fph, "#define XCCL4 0%o\n", ECCL4 + ESQR4);
  304.     fprintf(fph, "#define XCCL5 0%o\n", ECCL5 + ESQR5);
  305.     fprintf(fph, "#define XCCL6 0%o\n", ECCL6 + ESQR6);
  306.  
  307.  
  308.     mn1 = -o1;
  309.     mx1 = (int)(DEGTORAD * DRNG1 * RATIO1) - o1;
  310.     ORDER(mn1, mx1);
  311.     mn2 = -o2;
  312.     mx2 = (int)(DEGTORAD * DRNG2 * RATIO2) - o2;
  313.     ORDER(mn2, mx2);
  314.     mn3 = -o3;
  315.     mx3 = (int)(DEGTORAD * DRNG3 * RATIO3) - o3;
  316.     ORDER(mn3, mx3);
  317.     mn4 = -o4;
  318.     mx4 = (int)(DEGTORAD * DRNG4 * RATIO4) - o4;
  319.     ORDER(mn4, mx4);
  320.     mn5 = -o5;
  321.     mx5 = (int)(DEGTORAD * DRNG5 * RATIO5
  322.           + DEGTORAD * DRNG4 * RATI54) - o5;
  323.     ORDER(mn5, mx5);
  324.     mn6 = -o6;
  325.     mx6 = (int)(DEGTORAD * DRNG6 * RATIO6 + DEGTORAD * DRNG5 * RATI65
  326.           + DEGTORAD * DRNG4 * RATI64) - o6;
  327.     ORDER(mn6, mx6);
  328.  
  329.     fprintf(fph, "#define ECMN1 0%o\n", mn1);
  330.     fprintf(fph, "#define ECMN2 0%o\n", mn2);
  331.     fprintf(fph, "#define ECMN3 0%o\n", mn3);
  332.     fprintf(fph, "#define ECMN4 0%o\n", mn4);
  333.     fprintf(fph, "#define ECMN5 0%o\n", mn5);
  334.     fprintf(fph, "#define ECMN6 0%o\n", mn6);
  335.  
  336.     fprintf(fph, "#define ECMX1 0%o\n", mx1);
  337.     fprintf(fph, "#define ECMX2 0%o\n", mx2);
  338.     fprintf(fph, "#define ECMX3 0%o\n", mx3);
  339.     fprintf(fph, "#define ECMX4 0%o\n", mx4);
  340.     fprintf(fph, "#define ECMX5 0%o\n", mx5);
  341.     fprintf(fph, "#define ECMX6 0%o\n", mx6);
  342.  
  343.     fprintf(fph, "#define MXDC1 0%o\n", MXDC1);
  344.     fprintf(fph, "#define MXDC2 0%o\n", MXDC2);
  345.     fprintf(fph, "#define MXDC3 0%o\n", MXDC3);
  346.     fprintf(fph, "#define MXDC4 0%o\n", MXDC4);
  347.     fprintf(fph, "#define MXDC5 0%o\n", MXDC5);
  348.     fprintf(fph, "#define MXDC6 0%o\n", MXDC6);
  349.  
  350.     fprintf(fph, "#define MXOC1 0%o\n", MXOC1);
  351.     fprintf(fph, "#define MXOC2 0%o\n", MXOC2);
  352.     fprintf(fph, "#define MXOC3 0%o\n", MXOC3);
  353.     fprintf(fph, "#define MXOC4 0%o\n", MXOC4);
  354.     fprintf(fph, "#define MXOC5 0%o\n", MXOC5);
  355.     fprintf(fph, "#define MXOC6 0%o\n", MXOC6);
  356.  
  357.     fprintf(fph, "#define CP21\t%f\n", CV * -(C21));
  358.     fprintf(fph, "#define CP31\t%f\n", CV * -(C31));
  359.     fprintf(fph, "#define CP32\t%f\n", CV * -(C32));
  360.     fprintf(fph, "#define CP50\t%f\n", CV * -(C50));
  361. }
  362.