home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL091 < prev    next >
Encoding:
Text File  |  1987-03-02  |  1.5 KB  |  36 lines
  1. #define RD      57.29577951308232100
  2. main()
  3. {
  4.     int i[6];
  5.     short e[6];
  6.     double a[6], r[6];
  7.  
  8.     for (; ; ) {
  9.         printf("give 6 encoder values : ");
  10.         if (scanf("%d%d%d%d%d%d",i , i+1, i+2, i+3, i+4, i+5) != 6)
  11.             exit(0);
  12. e[0] = i[0]; e[1] = i[1]; e[2] = i[2]; e[3] = i[3]; e[4] = i[4]; e[5] = i[5];
  13.         enctorng(r, e);
  14. printf("enc > rng     %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f\n",
  15. r[0] * RD, r[1] * RD, r[2] * RD, r[3] * RD, r[4] * RD, r[5] * RD);
  16.         rngtoenc(e, r);
  17. printf("rng > enc     %-6d   %-6d   %-6d   %-6d   %-6d   %-6d\n",
  18. e[0] & 0xffff, e[1] & 0xffff, e[2] & 0xffff, e[3] & 0xffff, e[4] & 0xffff, e[5]
  19.         enctorng(r, e);
  20. printf("enc > rng     %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f\n",
  21. r[0] * RD, r[1] * RD, r[2] * RD, r[3] * RD, r[4] * RD, r[5] * RD);
  22.         enctoang(a, e);
  23. printf("enc > ang     %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f\n",
  24. a[0] * RD, a[1] * RD, a[2] * RD, a[3] * RD, a[4] * RD, a[5] * RD);
  25.         angtoenc(e, a);
  26. printf("ang > enc     %-6d   %-6d   %-6d   %-6d   %-6d   %-6d\n",
  27. e[0] & 0xffff, e[1] & 0xffff, e[2] & 0xffff, e[3] & 0xffff, e[4] & 0xffff, e[5]
  28.         enctoang(a, e);
  29. printf("enc > ang     %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f\n",
  30. a[0] * RD, a[1] * RD, a[2] * RD, a[3] * RD, a[4] * RD, a[5] * RD);
  31. printf("off           %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f   %-4.1f\n",
  32. (r[0] - a[0]) * RD, (r[1] - a[1]) * RD, (r[2] - a[2]) * RD,
  33. (r[3] - a[3]) * RD, (r[4] - a[4]) * RD, (r[5] - a[5]) * RD);
  34.     }
  35. }
  36.