home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl033 < prev    next >
Text File  |  1987-03-02  |  7KB  |  378 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : main.c
  7.  *      Remarks : From where everything begins, heavyly dependent on
  8.  *                the version, implements what is not needed from the
  9.  *                real time library. Opens and close files.
  10.  *                The real time library functions are replaced
  11.  *                by pseudo ones (manip.h _l) by macro process.
  12.  *      Usage   : part of the library
  13.  */
  14.  
  15. #include <ctype.h>
  16. #include <signal.h>
  17. #include "../h/which.h"
  18. #include "../h/switch.h"
  19. #include "../h/rccl.h"
  20. #include "../h/manip.h"
  21. #include "../h/kine.h"
  22. #include "../h/bio.h"
  23. #include "../h/umac.h"
  24.  
  25. #define RW      2
  26.  
  27. #ifdef FAKE
  28. static int pid, clockid;
  29. #endif
  30.  
  31.  
  32. main(argc, argv)
  33. int argc;
  34. char **argv;
  35. {
  36.     char st[80];
  37.     char *s, *stp, *strcat();
  38.     int q;
  39.     int fddbo;
  40.     bool prints_save;
  41.     char *mduper = "can't duplicate data base file\n";
  42.     char bd[BUFS];
  43.  
  44.     while(--argc > 0 && **++argv == '-') {
  45.         for(s = *argv+1; *s; s++) { /* scan multiple arg */
  46.             switch (*s) {
  47.             case 'v' : /* verbose */
  48.                 prints_out++;
  49.                 break;
  50.  
  51.             case 'D' : /* data base */
  52.                 ++s;
  53.                 for (stp = st; *s != '\0'; ) {
  54.                     *stp++ = *s++;
  55.                 }
  56.                 *stp = '\0';
  57.                 if ((fddb = open(st, RW)) < 0) {
  58.                     printf("%s does'nt exit, create ?", st);
  59.                     QUERY(q);
  60.                     if (q == 'y') {
  61.                         if ((fddb = makedb(st)) < 0) {
  62.                             exit(14);
  63.                         }
  64.                         if ((fddb = open(st, RW)) < 0) {
  65.                             exit(15);
  66.                         }
  67.                     }
  68.                 }
  69.                 else {
  70.                  (void) strcat(st, ".old");
  71.                     if ((fddbo = creat(st, 0644)) < 0) {
  72.                         printf(mduper);
  73.                         exit(16);
  74.                     }
  75.                     while ((q = read(fddb, bd, BUFS)) > 0) {
  76.                         if (write(fddbo, bd, q) < 0) {
  77.                             printf(mduper);
  78.                             exit(17);
  79.                         }
  80.                     }
  81.                 }
  82.                 --s;
  83.                 break;
  84.  
  85.             case 'b' : /* no force stuff */
  86.                 force_ctl = NO;
  87.                 break;
  88.  
  89.  
  90. #ifndef REAL    /* permit i/o s */
  91.             case 'g' : /* graphics */
  92.                 opsw_n.graphics = YES;
  93.                 break;
  94.  
  95.             case 'd' : /* output relative solution angles */
  96.                 opsw_n.numerics = YES;
  97.                 break;
  98.  
  99.             case 'a' : /* output solution angles */
  100.                 opsw_n.angles = YES;
  101.                 break;
  102.  
  103.             case 'k' : /* output t6 but not j6 */
  104.                 opsw_n.t6butnotj6 = YES;
  105.                 break;
  106.  
  107.             case 'e' : /* output encoder values */
  108.                 opsw_n.encoders = YES;
  109.                 break;
  110.  
  111.             default :
  112.                 printf(" options -v -g -d -a -k -e -b -D*\n");
  113.                 exit(33);
  114. #else
  115.             default :
  116.                 printf(" options -v -b -D*\n");
  117.                 exit(33);
  118. #endif
  119.             }
  120.         }
  121.     }
  122.     if (prints_out) {
  123.         if ((fpi = fdopen(creat("@@.out", 0644), "w")) == NULL) {
  124.             printf("can't create info file\n");
  125.             exit(22);
  126.         }
  127.     }
  128.  
  129.     prints_save = prints_out;
  130.     prints_out = NO;
  131.  
  132.     calibrate();
  133. #ifdef PLAN
  134.     startup();
  135.     movejnts(park, 100, 300);
  136. #else
  137.     movejnts(park, 100, 300);
  138.     startup();
  139. #endif
  140.     prints_out = prints_save;
  141. #ifdef PUMA
  142.     pumatask();
  143. #endif
  144. #ifdef STAN
  145.     stantask();
  146. #endif
  147.     stop(300);
  148.     waitfor(completed);
  149.     release("task ended");
  150. }
  151.  
  152. /*
  153.  * according to the cases perform initial actions
  154.  */
  155.  
  156. startup() /*::*/
  157. {
  158.     control(checkstate_n, setpoint_n);
  159. #ifdef FAKE
  160.     if ((clockid = vfork()) < 0) {
  161.         fprintf(stderr, "can't fork\n");
  162.         exit(13);
  163.     }
  164.     if (clockid == 0) {
  165.         execl("clock");
  166.         fprintf(stderr, "cant'n exec clock\n");
  167.         exit(13);
  168.     }
  169.     pid = getpid();
  170. #endif
  171. }
  172.  
  173. /*
  174.  * put process to sleep for 1/10 of a second
  175.  */
  176.  
  177. suspendfg() /*::*/
  178. {
  179. #ifdef PLAN
  180.     checkstate_n();
  181.     setpoint_n();
  182. #else
  183.     nap(6);
  184. #endif
  185. }
  186.  
  187.  
  188. /*
  189.  * software error routine
  190.  */
  191. #ifndef REAL
  192. /*ARGSUSED*/
  193. #endif
  194.  
  195. giveup(m, user)  /*::*/
  196. char *m;
  197. bool user;
  198. {
  199.     terminate = YES;
  200.     mess = m;
  201. #ifndef REAL
  202.     release("giveup");
  203.     exit(1);
  204. #else
  205.     if (user) {
  206.         release("giveup");
  207.         mess = "";
  208.         for (; ; ) {
  209.             pause();
  210.         }
  211.         /*NOTREACHED*/
  212.     }
  213. #endif
  214. }
  215.  
  216.  
  217. #ifndef REAL
  218. /*
  219.  * replace rtc stuff by internal functions
  220.  */
  221.  
  222. release(s) /*:: quit interrupt routine */
  223. char *s;
  224. {
  225. #ifdef FAKE
  226.  (void) signal(SIGCLCK, SIG_IGN);
  227.  (void) kill(clockid, SIGKILL);
  228. #endif
  229.     if (opsw_n.encoders) {
  230.         Close(&iobf_n[EB]);
  231.     }
  232.     if (opsw_n.graphics) {
  233.         Close(&iobf_n[J1]);
  234.         Close(&iobf_n[J2]);
  235.         Close(&iobf_n[J3]);
  236.         Close(&iobf_n[J4]);
  237.         Close(&iobf_n[J5]);
  238.         Close(&iobf_n[J6]);
  239.         if (opsw_n.t6butnotj6) {
  240.             Close(&iobf_n[6]);
  241.             Close(&iobf_n[7]);
  242.             Close(&iobf_n[8]);
  243.             Close(&iobf_n[9]);
  244.             Close(&iobf_n[10]);
  245.             Close(&iobf_n[11]);
  246.         }
  247.         Close(&iobf_n[TB]);
  248.         Close(&iobf_n[CB]);
  249.     }
  250.     if (opsw_n.numerics) {
  251.      (void) fclose(fpo);
  252.     }
  253.     if (prints_out) {
  254.      (void) fclose(fpi);
  255.     }
  256.     if ((int)s == SIGINT) {
  257.         printf("time %d : Interrupted\n", rtime);
  258.         exit(1);
  259.     }
  260.     else {
  261.         printf("%s\n", mess);
  262.         printf("pos %s time %d : %s\n", goalpos->name, rtime, s);
  263.     }
  264.     exit(0);
  265. }
  266.  
  267.  
  268. /*
  269.  * Flush a buffer
  270.  */
  271.  
  272. static Close(b) /*##*/
  273. BIO_PTR b;
  274. {
  275.     int n;
  276.  
  277.     if (b->ptr != NULL) {
  278.         n = b->ptr - b->buf;
  279.         if (write(b->fd, b->buf, n) != n){
  280.             fprintf(stderr, "write io error\n");
  281.             exit(2);
  282.         }
  283.     }
  284.     if (close(b->fd) < 0) {
  285.         fprintf(stderr, "close io error\n");
  286.         exit(2);
  287.     }
  288. }
  289.  
  290.  
  291. #ifdef FAKE
  292. static int (* fne1)();
  293. static int (* fne2)();
  294.  
  295.  
  296. static dummy() /*##*/
  297. {
  298.  (void) signal(SIGCLCK, dummy);
  299. }
  300.  
  301. static onintr() /*##*/
  302. {
  303.  (void) signal(SIGCLCK, dummy);
  304.     (* fne1)();
  305.     (* fne2)();
  306.  (void) signal(SIGCLCK, onintr);
  307. }
  308. #endif
  309.  
  310. #ifdef PLAN
  311. /*ARGSUSED*/
  312. #endif
  313.  
  314. control(fn1, fn2) /*::*/
  315. int (* fn1)(), (* fn2)();
  316. {
  317.  
  318.     if (opsw_n.numerics)  {
  319.         if ((fpo = fdopen(creat("@.out", 0644), "w")) == NULL) {
  320.             printf("can't open data file\n");
  321.             exit(22);
  322.         }
  323.     }
  324.  
  325.     if (opsw_n.graphics) {
  326.         iobf_n[J1].fd = creat("../g/f1.out", 0644);
  327.         iobf_n[J2].fd = creat("../g/f2.out", 0644);
  328.         iobf_n[J3].fd = creat("../g/f3.out", 0644);
  329.         iobf_n[J4].fd = creat("../g/f4.out", 0644);
  330.         iobf_n[J5].fd = creat("../g/f5.out", 0644);
  331.         iobf_n[J6].fd = creat("../g/f6.out", 0644);
  332.         if (opsw_n.t6butnotj6) {
  333.             iobf_n[6].fd = creat("../g/f7.out", 0644);
  334.             iobf_n[7].fd = creat("../g/f8.out", 0644);
  335.             iobf_n[8].fd = creat("../g/f9.out", 0644);
  336.             iobf_n[9].fd = creat("../g/f10.out", 0644);
  337.             iobf_n[10].fd = creat("../g/f11.out", 0644);
  338.             iobf_n[11].fd = creat("../g/f12.out", 0644);
  339.         }
  340.         iobf_n[TB].fd = creat("../g/t.out", 0644);
  341.         iobf_n[CB].fd = creat("../g/c.out", 0644);
  342.     }
  343.     if (iobf_n[J1].fd < 0 || iobf_n[J2].fd < 0 || iobf_n[J3].fd < 0 ||
  344.         iobf_n[J4].fd < 0 || iobf_n[J5].fd < 0 || iobf_n[J6].fd < 0 ||
  345.         iobf_n[6].fd < 0 || iobf_n[7].fd < 0 || iobf_n[8].fd < 0 ||
  346.         iobf_n[9].fd < 0 || iobf_n[10].fd < 0 || iobf_n[11].fd < 0 ||
  347.         iobf_n[TB].fd < 0 || iobf_n[CB].fd < 0) {
  348.         printf("can't create graphic files\n");
  349.         exit(22);
  350.     }
  351.     if (opsw_n.encoders) {
  352.         if ((iobf_n[EB].fd = creat("@@@.out",0644)) < 0) {
  353.             printf("can't create encoder file\n");
  354.             exit(22);
  355.         }
  356.     }
  357.  (void) signal(SIGINT, release);
  358. #ifdef FAKE
  359.     fne1 = fn1;
  360.     fne2 = fn2;
  361.  (void) signal(SIGCLCK, onintr);
  362. #endif
  363. }
  364. #endif
  365.  
  366.  
  367.  
  368. static calibrate() /*##*/
  369. {
  370.     assignjs_n(j6, &jcal_c);
  371.     solveconf_n(j6);
  372.     jns_to_tr_n(rest, j6, YES);
  373.     park = makeposition("PARK", t6, EQ, rest, TL, t6);
  374.     lastpos = goalpos = park;
  375.     Assigntr(here, rest);
  376.     there = makeposition("THERE", t6, EQ, here, TL, t6);
  377. }
  378.