home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT03.ZIP / RCCL152 < prev    next >
Encoding:
Text File  |  1987-03-02  |  5.6 KB  |  166 lines
  1. /*
  2.  * RTC  Version 2.0           Author :  Vincent Hayward
  3.  * MAC          2.0                     School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : h
  6.  *      File    : pumac.c
  7.  *      Remarks : generates the include file pumaload.h containing all the
  8.  *                precalculated friction constants of the puma 600.
  9.  *      Usage   : cc pumal.c -lm ; a.out
  10.  */
  11.  
  12. /*
  13.  * friction and gravity loadings for PUMA 600 robot
  14.  * installed in Potter Building, room B20.
  15.  * After Zhang Hong (TR..., spring, summer 1983)
  16.  *
  17.  * Joints 1, 3    have a positive velocity when encoder values decrease.
  18.  * .......2 4 5 6.produce positive torque for negative current
  19.  *
  20.  *
  21.  * torque to dac mapping:
  22.  * dac = t * TDPA + TDPB  (+)
  23.  * dac = t * TDNA + TDNB  (-)
  24.  *
  25.  * Those coeff are first determined by inverting the relations above.
  26.  *
  27.  * adc to torque mapping:
  28.  * t = adc * ATPA + ATPB  (+)
  29.  * t = adc * ATNA + ATNB  (-)
  30.  *
  31.  * dac to adc mapping:
  32.  * dac = ALP adc + BET
  33.  *
  34.  * All values in dac, adc, N-mm        (from measurements in oz-in)
  35.  */
  36.  
  37.  
  38. #include <stdio.h>
  39.  
  40. #define PA1     (9.0623652 )
  41. #define NA1     (8.6431404 )
  42. #define PA2     (13.4400000)
  43. #define NA2     (14.1120000)
  44. #define PA3     (7.2788060 )
  45. #define NA3     (7.0163582 )
  46. #define PA4     (1.7217002 )
  47. #define NA4     (1.7705593 )
  48. #define PA5     (1.4585102 )
  49. #define NA5     (1.4546531 )
  50. #define PA6     (1.8182966 )
  51. #define NA6     (1.7624517 )
  52.  
  53. #define PB1     (-2016.536 )
  54. #define NB1     (1800.536  )
  55. #define PB2     (-1500.0   )    /*(-1088.000 )*/
  56. #define NB2     (1500.0    )    /*(1088.00   )*/
  57. #define PB3     (-632.846  )
  58. #define NB3     (631.385   )
  59. #define PB4     (-116.0    )
  60. #define NB4     (115.0     )
  61. #define PB5     (-100.648  )
  62. #define NB5     (146.648   )
  63. #define PB6     (-90.0     )
  64. #define NB6     (88.0      )
  65.  
  66. #define ALP1    (-5.8)
  67. #define BET1    (-80.)
  68. #define ALP2    (-5.8)
  69. #define BET2    (-80.)
  70. #define ALP3    (-5.8)
  71. #define BET3    (-80.)
  72. #define ALP4    (-5.8)
  73. #define BET4    (-80.)
  74. #define ALP5    (-5.8)
  75. #define BET5    (-80.)
  76. #define ALP6    (-5.8)
  77. #define BET6    (-80.)
  78.  
  79.  
  80.  
  81. #define CV      7.05       /* oz in -> N mm */
  82. #define ONE     1.
  83. #define TWO     2.
  84.  
  85. main()
  86. {
  87.     FILE *fp = fdopen(creat("pumaload.h",  0644),"w");
  88.     double pa1, pb1, na1, nb1;
  89.     double pa2, pb2, na2, nb2;
  90.     double pa3, pb3, na3, nb3;
  91.     double pa4, pb4, na4, nb4;
  92.     double pa5, pb5, na5, nb5;
  93.     double pa6, pb6, na6, nb6;
  94.  
  95.  
  96.     fprintf(fp, "#define TDPA1\t%f\n", pa1 =  ONE / (PA1 * CV));
  97.     fprintf(fp, "#define TDPB1\t%f\n", pb1 = -(PB1) / (PA1));
  98.  
  99.     fprintf(fp, "#define TDPA2\t%f\n", pa2 = -ONE / (PA2 * CV));
  100.     fprintf(fp, "#define TDPB2\t%f\n", pb2 =  (PB2) / (PA2));
  101.  
  102.     fprintf(fp, "#define TDPA3\t%f\n", pa3 =  ONE / (PA3 * CV));
  103.     fprintf(fp, "#define TDPB3\t%f\n", pb3 = -(PB3) / (PA3));
  104.  
  105.     fprintf(fp, "#define TDPA4\t%f\n", pa4 = -ONE / (PA4 * CV));
  106.     fprintf(fp, "#define TDPB4\t%f\n", pb4 =  (PB4) / (PA4));
  107.  
  108.     fprintf(fp, "#define TDPA5\t%f\n", pa5 = -ONE / (PA5 * CV));
  109.     fprintf(fp, "#define TDPB5\t%f\n", pb5 =  (PB5) / (PA5));
  110.  
  111.     fprintf(fp, "#define TDPA6\t%f\n", pa6 = -ONE / (PA6 * CV));
  112.     fprintf(fp, "#define TDPB6\t%f\n", pb6 =  (PB6) / (PA6));
  113.  
  114.     fprintf(fp, "#define TDNA1\t%f\n", na1 =  ONE / (NA1 * CV));
  115.     fprintf(fp, "#define TDNB1\t%f\n", nb1 = -(NB1) / (NA1));
  116.  
  117.     fprintf(fp, "#define TDNA2\t%f\n", na2 = -ONE / (NA2 * CV));
  118.     fprintf(fp, "#define TDNB2\t%f\n", nb2 =  (NB2) / (NA2));
  119.  
  120.     fprintf(fp, "#define TDNA3\t%f\n", na3 =  ONE / (NA3 * CV));
  121.     fprintf(fp, "#define TDNB3\t%f\n", nb3 =  -(NB3) / (NA3));
  122.  
  123.     fprintf(fp, "#define TDNA4\t%f\n", na4 =  -ONE / (NA4 * CV));
  124.     fprintf(fp, "#define TDNB4\t%f\n", nb4 =  (NB4) / (NA4));
  125.  
  126.     fprintf(fp, "#define TDNA5\t%f\n", na5 =  -ONE / (NA5 * CV));
  127.     fprintf(fp, "#define TDNB5\t%f\n", nb5 =  (NB5) / (NA5));
  128.  
  129.     fprintf(fp, "#define TDNA6\t%f\n", na6 = -ONE / (NA6 * CV));
  130.     fprintf(fp, "#define TDNB6\t%f\n", nb6 =  (NB6) / (NA6));
  131.  
  132.     fprintf(fp, "#define TDA1\t%f\n",  TWO / ((PA1 + NA1) * CV));
  133.     fprintf(fp, "#define TDA2\t%f\n", -TWO / ((PA2 + NA2) * CV));
  134.     fprintf(fp, "#define TDA3\t%f\n",  TWO / ((PA3 + NA3) * CV));
  135.     fprintf(fp, "#define TDA4\t%f\n", -TWO / ((PA4 + NA4) * CV));
  136.     fprintf(fp, "#define TDA5\t%f\n", -TWO / ((PA5 + NA5) * CV));
  137.     fprintf(fp, "#define TDA6\t%f\n", -TWO / ((PA6 + NA6) * CV));
  138.  
  139.  
  140.     fprintf(fp, "#define ATPA1\t%f\n", ALP1 / pa1);
  141.     fprintf(fp, "#define ATPA2\t%f\n", ALP2 / pa2);
  142.     fprintf(fp, "#define ATPA3\t%f\n", ALP3 / pa3);
  143.     fprintf(fp, "#define ATPA4\t%f\n", ALP4 / pa4);
  144.     fprintf(fp, "#define ATPA5\t%f\n", ALP5 / pa5);
  145.     fprintf(fp, "#define ATPA6\t%f\n", ALP6 / pa6);
  146.     fprintf(fp, "#define ATNA1\t%f\n", ALP1 / na1);
  147.     fprintf(fp, "#define ATNA2\t%f\n", ALP2 / na2);
  148.     fprintf(fp, "#define ATNA3\t%f\n", ALP3 / na3);
  149.     fprintf(fp, "#define ATNA4\t%f\n", ALP4 / na4);
  150.     fprintf(fp, "#define ATNA5\t%f\n", ALP5 / na5);
  151.     fprintf(fp, "#define ATNA6\t%f\n", ALP6 / na6);
  152.  
  153.     fprintf(fp, "#define ATPB1\t%f\n", (BET1 - pb1) / pa1);
  154.     fprintf(fp, "#define ATPB2\t%f\n", (BET2 - pb2) / pa2);
  155.     fprintf(fp, "#define ATPB3\t%f\n", (BET3 - pb3) / pa3);
  156.     fprintf(fp, "#define ATPB4\t%f\n", (BET4 - pb4) / pa4);
  157.     fprintf(fp, "#define ATPB5\t%f\n", (BET5 - pb5) / pa5);
  158.     fprintf(fp, "#define ATPB6\t%f\n", (BET6 - pb6) / pa6);
  159.     fprintf(fp, "#define ATNB1\t%f\n", (BET1 - nb1) / na1);
  160.     fprintf(fp, "#define ATNB2\t%f\n", (BET2 - nb2) / na2);
  161.     fprintf(fp, "#define ATNB3\t%f\n", (BET3 - nb3) / na3);
  162.     fprintf(fp, "#define ATNB4\t%f\n", (BET4 - nb4) / na4);
  163.     fprintf(fp, "#define ATNB5\t%f\n", (BET5 - nb5) / na5);
  164.     fprintf(fp, "#define ATNB6\t%f\n", (BET6 - nb6) / na6);
  165. }
  166.