home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl084 < prev    next >
Text File  |  1987-03-02  |  3KB  |  168 lines
  1. /*
  2.  * RTC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : rtc
  6.  *      File    : tfree.c
  7.  *      Remarks : Utility, Force control testing.
  8.  *                better not run with the wrist free.
  9.  *      Usage   : cc tfree.c rtc.a -lnm; a.out
  10.  */
  11.  
  12. #include "../h/rtc.h"
  13. #include "../h/pumadata.h"
  14.  
  15. #define DEGTORAD        0.01745329251994330
  16.  
  17. extern struct how how;
  18. extern struct chg chg;
  19.  
  20. main()
  21. {
  22.     extern int terminate;
  23.     int dummy(), freej();
  24.  
  25.     control(dummy, freej);
  26.     for (; ; ) {
  27.         nap(10);
  28.         printf("%6d %6d %6d %6d %6d %6d\n",
  29. how.adcr[0], how.adcr[1], how.adcr[2], how.adcr[3], how.adcr[4], how.adcr[5]);
  30.     }
  31. }
  32.  
  33.  
  34. dummy(){}
  35.  
  36. static freej()
  37. {
  38.     double sin(), cos();
  39.  
  40.     static int first = YES;
  41.     static unsigned short old[NJOINTS];
  42.     static int shake[6] = {0, 0, 0, 0, 0, 0};
  43.     short cur[NJOINTS];
  44.     double jpos[NJOINTS];
  45.     double jrng[NJOINTS];
  46.     double gtor[NJOINTS];
  47.     double c2, c23, s23, c4, s4, c5, s5;
  48.     double bound = 10. * DEGTORAD;
  49.     register int i;
  50.  
  51.     if (first) {
  52.         first = NO;
  53.         for (i = 0; i < NJOINTS; ++i) {
  54.             old[i] = how.pos[i];
  55.         }
  56.         chg.g_rate.valg = 1;
  57.         chg.g_rate.set = YES;
  58.         return;
  59.     }
  60.  
  61.     enctorng(jrng, how.pos);
  62.     rngtoang(jpos, jrng);
  63.  
  64.     c2 = cos(jpos[1]);
  65.     c23 = cos(jpos[1] + jpos[2]);
  66.     s23 = sin(jpos[1] + jpos[2]);
  67.     c4 = cos(jpos[3]);
  68.     s4 = sin(jpos[3]);
  69.     c5 = cos(jpos[4]);
  70.     s5 = sin(jpos[4]);
  71.     gravload(gtor, c2, c23, s23, c4, s4, c5, s5);
  72.  
  73.     tortodac(cur, gtor, how.pos, old);
  74.  
  75.     for (i = 0; i < 6; ++i) {
  76.         cur[i] += shake[i];
  77.         shake[i] = -shake[i];
  78.     }
  79.  
  80.     if (jrng[0] < bound) {
  81.         cur[0] += 500;
  82.     }
  83.     if (jrng[0] > JRNG1 - bound) {
  84.         cur[0] += -500;
  85.     }
  86.  
  87.     if (jrng[1] < bound) {
  88.         cur[1] += -500;
  89.     }
  90.     if (jrng[1] > JRNG2 - bound) {
  91.         cur[1] += 500;
  92.     }
  93.  
  94.     if (jrng[2] < bound) {
  95.         cur[2] += 500;
  96.     }
  97.     if (jrng[2] > JRNG3 - bound) {
  98.         cur[2] += -500;
  99.     }
  100.  
  101.     if (jrng[3] < bound) {
  102.         cur[3] += -400;
  103.     }
  104.     if (jrng[3] > JRNG4 - bound) {
  105.         cur[3] += 400;
  106.     }
  107.  
  108.     if (jrng[4] < bound) {
  109.         cur[4] += -400;
  110.     }
  111.     if (jrng[4] > JRNG5 - bound) {
  112.         cur[4] += 400;
  113.     }
  114.  
  115.     if (jrng[5] < bound) {
  116.         cur[5] += -400;
  117.     }
  118.     if (jrng[5] > JRNG6 - bound) {
  119.         cur[5] += 400;
  120.     }
  121.  
  122.     chg.i_motion[0].set = CUR;
  123.     chg.i_motion[0].vali = cur[0];
  124.  
  125.     chg.i_motion[1].set = CUR;
  126.     chg.i_motion[1].vali = cur[1];
  127.  
  128.     chg.i_motion[2].set = CUR;
  129.     chg.i_motion[2].vali = cur[2];
  130.  
  131. /*
  132.  *  Here i broke the puma's wrist because it bang into limit
  133.  */
  134.  
  135. /* Suppression....
  136.  
  137.     chg.i_motion[3].set = CUR;
  138.     chg.i_motion[3].vali = cur[3];
  139.  
  140.     chg.i_motion[4].set = CUR;
  141.     chg.i_motion[4].vali = cur[4];
  142.  
  143.     chg.i_motion[5].set = CUR;
  144.     chg.i_motion[5].vali = cur[5];
  145.  
  146. till here */
  147.  
  148.     for (i = 0; i < NJOINTS; ++i) {
  149.         old[i] = how.pos[i];
  150.     }
  151. }
  152.  
  153.  
  154.  
  155.  
  156. gravload(l, c2, c23, s23, c4, s4, c5, s5) /*::*/
  157. register
  158. double *l;
  159. double c2, c23, s23, c4, s4, c5, s5;
  160. {
  161.     l[0] = 0.;
  162.     l[2] = CP32 * s23 + CP31 * c23;
  163.     l[1] = l[2] + CP21 * c2;
  164.     l[3] = -(CP50 * s23 * s4 * s5);
  165.     l[4] = CP50 * (s23 * c4 * c5 + c23 * s5);
  166.     l[5] = 0.;
  167. }
  168.