home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL083 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.5 KB  |  81 lines
  1. /*
  2.  * RTC  Version 2.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : rtc
  6.  *      File    : random.c
  7.  *      Remarks : Utility, for tests
  8.  *      Usage   : cc random.c rtc.a; a.out
  9.  */
  10.  
  11. #include "../h/rtc.h"
  12. #include "../h/pumadata.h"
  13.  
  14. #define ACC     6
  15.  
  16. main()
  17. {
  18.     extern int terminate;
  19.     int dummy(), back();
  20.  
  21.     stepmode();
  22.  
  23.     control(dummy, back);
  24.     for (; ; )
  25.         pause();
  26. }
  27.  
  28.  
  29. static back()
  30. {
  31.     extern struct how how;
  32.     extern struct chg chg;
  33.     extern int terminate;
  34.     static int first = YES;
  35.     static unsigned short   where[NJOINTS],
  36.                 inc[NJOINTS],
  37.                 half[NJOINTS],
  38.                 cal[NJOINTS] =
  39.             {ECCL1, ECCL2, ECCL3, ECCL4, ECCL5, ECCL6};
  40.     register int i;
  41.  
  42.     if (first) {
  43.         first = NO;
  44.         for (i = 0; i < NJOINTS; ++i) {
  45.             where[i] = how.pos[i];
  46.             if (where[i] > cal[i]) {
  47.                 half[i] = (where[i] - cal[i]) / 2 + cal[i];
  48.             }
  49.             else {
  50.                 half[i] = cal[i] - (cal[i] - where[i]) / 2;
  51.             }
  52.             inc[i] = 0;
  53.         }
  54.         return;
  55.     }
  56.     for (i = 0; i < NJOINTS; ++i) {
  57.         if (where[i] > cal[i]) {
  58.             if (where[i] > half[i]) {
  59.                   inc[i] -= ACC;
  60.             }
  61.             else {
  62.                   inc[i] += ACC;
  63.             }
  64.         }
  65.         if (where[i] < cal[i]) {
  66.             if (where[i] < half[i]) {
  67.                   inc[i] += ACC;
  68.             }
  69.             else {
  70.                   inc[i] -= ACC;
  71.             }
  72.         }
  73.         where[i] += inc[i];
  74.         chg.a_motion.vala[i] = where[i];
  75.     }
  76.     chg.a_motion.set = POS;
  77. }
  78.  
  79.  
  80. dummy(){}
  81.