home *** CD-ROM | disk | FTP | other *** search
/ ftp.hitl.washington.edu / ftp.hitl.washington.edu.tar / ftp.hitl.washington.edu / pub / people / habib / kodak / #spaceorb.cc# next >
Text File  |  2000-04-18  |  12KB  |  322 lines
  1. #include <stdio.h>
  2. //#include <windows.h>
  3. #include <iostream.h>
  4. //#include <winbase.h>
  5. #include <math.h>
  6. #include <time.h>
  7.  
  8. static int sorb_trans_thresh[3] = {0,0,0};
  9. static int sorb_rot_thresh[3] = {0,0,0};
  10.  
  11. int t[3],r[3];
  12.  
  13. int COM_NOT_INITIALIZED = 1;
  14. WORD num; 
  15. HANDLE comHandle, KBD;
  16. /*
  17.   Convert 8 1/2 7bit bytes into 6 ten-bit integers  
  18.  
  19.     c[0]    c[1]    c[2]    c[3]    c[4]    c[5]    c[6]    c[7]    c[8]
  20. xdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxddddddd 
  21. xdddddddxddd           dxdddddddxdd           ddxdddddddxd
  22.       t[0]  ddddxdddddd       t[2] dddddxddddd      r[1]  ddddddxdddd ddd 
  23.                   t[1]                   r[0]                   r[2]
  24.  
  25. */
  26. inline void sorb_bits_convert( char cc[19])
  27. {
  28.     int i;
  29.     int c[19];
  30.     char SpaceWare[] = "SpaceWare!";
  31.  
  32.     /* Strip the MSB, which is a parity bit */
  33.     for (i = 0; i < 9; i++) {
  34.     c[i]  = cc[i];
  35.     c[i] &= 0177;        /* Make sure everything is 7bit */
  36.     c[i] ^= SpaceWare[i];    /* What's this doing in the data? */
  37.     }
  38.  
  39.     //cout << "                          " << c[7] <<"          " << c[8] << endl;
  40.  
  41.     /* Turn chars into 10 bit integers */
  42.     t[0] = ((c[0] & 0177)<<3)|((c[1] & 0160)>>4);
  43.     t[1] = ((c[1] & 0017)<<6)|((c[2] & 0176)>>1);
  44.     t[2] = ((c[2] & 0001)<<9)|((c[3] & 0177)<<2)|((c[4] & 0140)>>5);
  45.     r[0] = ((c[4] & 0037)<<5)|((c[5] & 0174)>>2);
  46.     r[1] = ((c[5] & 0003)<<8)|((c[6] & 0177)<<1)|((c[7] & 0100)>>6);
  47.     r[2] = ((c[7] & 0077)<<4)|((c[8] & 0170)>>3);
  48.  
  49.     /* Get the sign right. */
  50.     for(i = 0; i < 3; i ++) {
  51.     if (t[i] > 511) t[i] -= 1024;
  52.     if (fabs(t[i]) < sorb_trans_thresh[i]) t[i] = 0;
  53.     }
  54.     for(i = 0; i < 3; i ++) {
  55.     if (r[i] > 511) r[i] -= 1024;
  56.     if (fabs(r[i]) < sorb_rot_thresh[i]) r[i] = 0;
  57.     }
  58. }
  59.  
  60. initialize_com_port()
  61.  
  62. {    
  63.     DCB dcb;
  64.     COMMTIMEOUTS timeouts;
  65.     int success;
  66.     /*hFile = CreateFile ("COM2", GENERIC_READ ,FILE_SHARE_READ, NULL, 
  67.             CREATE_ALWAYS, FILE_FLAG_NO_BUFFERING, NULL); */
  68.  
  69.     // open COM port as a file (Handle.).
  70.     comHandle = CreateFile ("COM1", GENERIC_READ|GENERIC_WRITE, 0,NULL, 
  71.                             OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL,0);
  72.     if (comHandle == INVALID_HANDLE_VALUE)
  73.                                         cout << "comHandle failed" << endl;
  74.     KBD = GetStdHandle(STD_INPUT_HANDLE);
  75.         GetCommState(KBD,&dcb);
  76.         GetCommState(comHandle, &dcb);
  77.  
  78.  
  79.         //Get the current settings of the COMM port
  80.         success = GetCommState(comHandle, &dcb);
  81.         if(!success)
  82.                 cout << "GetCommState failed" << endl;
  83.  
  84.         //Modify the baud rate, et al.
  85.         dcb.BaudRate = 9600;
  86.         dcb.ByteSize = 8;
  87.         dcb.Parity = NOPARITY;
  88.         dcb.StopBits = ONESTOPBIT;
  89.  
  90.         //Apply the new comm port settings
  91.         success = SetCommState(comHandle, &dcb);
  92.         if(!success)
  93.                 cout << "SetCommState failed" << endl;
  94.  
  95.         //Change the ReadIntervalTimeout so that ReadFile will return
  96.         //immedialty.  See help file.
  97.         timeouts.ReadIntervalTimeout = MAXDWORD;
  98.         timeouts.ReadTotalTimeoutMultiplier = 0;
  99.         timeouts.ReadTotalTimeoutConstant = 0;
  100.         timeouts.WriteTotalTimeoutMultiplier = 0;
  101.         timeouts.WriteTotalTimeoutConstant = 0;
  102.         SetCommTimeouts(comHandle, &timeouts);
  103.  
  104.         //Set the Data Terminal Ready line
  105.         EscapeCommFunction(comHandle, SETDTR);       
  106.  
  107.         COM_NOT_INITIALIZED = 0;
  108. }
  109.  
  110. int read_orb(int *tt, int *rr)
  111. {
  112.     clock_t t1, t2;
  113.     // t1 = clock();
  114.     if (COM_NOT_INITIALIZED) initialize_com_port();
  115.     t[0] = t[1] = t[2] = r[0] = r[1] = r[2] = 0;
  116.     int Button_Number = 0;
  117.  
  118.     char str[100];
  119.     char c[19];
  120.     
  121.     ReadFile (comHandle, str, 1, &num, NULL);
  122.     if (num == 0) return -1;
  123.     str[0] &= 0177;
  124.     if (str[0] == 'D') {
  125.         num = 0;
  126.         while (num!=1)
  127.             ReadFile (comHandle, str, 1, &num, NULL); //cout << "read : " << num << endl;
  128.         // Sleep(20);
  129.         // ReadFile (comHandle, c  , 9, &num, NULL);
  130.         num = 0;
  131.         while (num!=1)
  132.             ReadFile (comHandle, &c[0]  , 1, &num, NULL); 
  133.         num = 0;
  134.         while (num!=1)
  135.             ReadFile (comHandle, &c[1]  , 1, &num, NULL); 
  136.         num = 0;
  137.         while (num!=1)
  138.             ReadFile (comHandle, &c[2]  , 1, &num, NULL); 
  139.         num = 0;
  140.         while (num!=1)
  141.             ReadFile (comHandle, &c[3]  , 1, &num, NULL); 
  142.         num = 0;
  143.         while (num!=1)
  144.             ReadFile (comHandle, &c[4]  , 1, &num, NULL); 
  145.         num = 0;
  146.         while (num!=1)
  147.             ReadFile (comHandle, &c[5]  , 1, &num, NULL); 
  148.         num = 0;
  149.         while (num!=1)
  150.             ReadFile (comHandle, &c[6]  , 1, &num, NULL); 
  151.         num = 0;
  152.         while (num!=1)
  153.             ReadFile (comHandle, &c[7]  , 1, &num, NULL); 
  154.         num = 0;
  155.         while (num!=1)
  156.             ReadFile (comHandle, &c[8]  , 1, &num, NULL); 
  157.         num = 0;
  158.          while (num!=1)
  159.             ReadFile (comHandle, str, 1, &num, NULL); 
  160.         sorb_bits_convert(c);
  161.         tt[0] = t[0]; tt[1] = t[1]; tt[2] = t[2];
  162.         rr[0] = r[0]; rr[1] = r[1]; rr[2] = r[2];
  163.         Button_Number = 995;
  164.         //t2 = clock();
  165.         //cout <<"time " << t2-t1 << endl;
  166.         //cout<<tt[0]<<" "<<tt[1]<<" "<<tt[2]<<" "<<rr[0]<<" "<<rr[1]<<" "<<rr[2]<<endl;
  167.     }
  168.     else
  169.         if (str[0] == 'K') {
  170.             ReadFile (comHandle, str, 5, &num, NULL);
  171.             str[0] &=0177;
  172.             str[1] &=0177;
  173.             if (str[1] == 0) return -1;
  174. /*            if (str[1] !=0){
  175.                 cout <<(int) str[0] << " milliseconds" <<endl;
  176.                 cout <<(int) str[1] << " button number"<<endl<<endl; 
  177.                 
  178.             } */
  179.             Button_Number = str[1];
  180.         }
  181.         else return -2;
  182. return Button_Number;
  183. }
  184.  
  185. /* 
  186. The orb's packet structure:
  187.  
  188. The very first byte of a newly initialized orb is `\r'. After this, the orb will send three kind of packets: 
  189.  
  190.    1.A one time only greeting giving version info and SpaceTec's trade mark. This, I call an `R' packet. (For why, see below.) 
  191.  
  192.    2.A button press/release with out any orb motion. This is a `K' packet.
  193.  
  194.    3.Orb motion with current button state. This is a `D' packet.
  195.  
  196. These packets are usually delimited by a Carriage Return. However, sometimes there will be more than one packet before a
  197. terminating CR. Also, usually it will take more than one call to read(2) before an entire packet is read from the serial port.
  198.  
  199. The packets are of, so called, type `R', `K' or `D' because their first byte is one of these values when printed as ASCII characters.
  200. It is this byte that can be used to anticipate what will follow, when reading in the packet.
  201.  
  202. The structure of each packet type is expanded on below: 
  203.  
  204. `R' packet 
  205.      The greeting packet is 52 bytes long, including the terminating CR. For instance, my orb prints: 
  206.  
  207.                           R Spaceball (R) V4.26 28-Jun-96 Copyright (C) 1996z\r
  208.  
  209.      The `R' is the packet flag. 
  210.  
  211. `K' packet 
  212.      The button press/release packet is a 6 byte packet, including the terminating CR. This packet contains information about a
  213.      button press or release event. There is no information about motion. It's flag is 75, or ASCII `K'. The values of the packet
  214.      are as such: 
  215.  
  216.                                  [`K'] [t] [buttons] [0] [??check sum??]
  217.  
  218.      Each element in a [ ] pair is a byte. Here are the meanings: 
  219.  
  220.      [`K']
  221.  
  222.           The packet flag.
  223.  
  224.      [t]
  225.  
  226.           The time, in milliseconds from the last button event. Values are 0 - 100, although, usually > 3. If there is no button
  227.           press in 100 ms, a packet showing current button state and 100 ms elapsed time will be sent.
  228.  
  229.      [buttons]
  230.  
  231.           The state of the buttons just before, and triggering, the packet. The state of buttons is an OR of all buttons pressed.
  232.           The value of individual buttons are as follows:
  233.  
  234.                0: No buttons 
  235.                1: Button A 
  236.                2: Button B 
  237.                4: Button C 
  238.                8: Button D 
  239.                16: Button E 
  240.                32: Button F 
  241.                64: Reset Button. (This button is on the bottom and will reset the current orb position to zero.) 
  242.  
  243.      [0]
  244.  
  245.           This byte is always zero, as far as I can tell. Maybe reserved for future expansion. 
  246.  
  247.      [??check sum??]
  248.  
  249.           This byte appears to be a check sum, although I haven't really looked into this. (Anyone?) 
  250.  
  251. `D' packet 
  252.      The motion packet has been the most difficult packet to figure out. This is mostly due to the fact that the 6 degrees of motion
  253.      data is encoded into 10 bits spread out over 9 bytes. To further complicate matters, the string ``SpaceWare'' is XORed into
  254.      the data. However, armed with this information and a bit of bit twiddling, one can gain access to the motion packet. 
  255.  
  256.      The packet is laid out like this: 
  257.  
  258.                            [`D] [buttons] [(9 bytes of data)] [??check sum??]
  259.  
  260.      Here are the explanations of the individual bytes. 
  261.  
  262.      [`D']
  263.  
  264.           The packet flag.
  265.  
  266.      [buttons]
  267.  
  268.           The state of the buttons. See above for button explanation. If this state changes while there is motion, it will trigger a
  269.           `K' packet.
  270.  
  271.      [(9 bytes of motion data)]
  272.  
  273.           This is the meat of the packet. It contains 9 7bit bytes, with the MSB unneeded. To turn these into translations and
  274.           rotations, one needs to XOR these 9 bytes with the 9 characters from the string ``SpaceWare'', ignore the MSB of
  275.           each bytes and strip off 10 bits for each d.o.f. Here is an attempt to show the structure: 
  276.  
  277.               c[0]    c[1]    c[2]    c[3]    c[4]    c[5]    c[6]    c[7]    c[8]
  278.           xdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxddddddd 
  279.           xdddddddxddd           dxdddddddxdd           ddxdddddddxd
  280.                 t[0]  ddddxdddddd       t[2] dddddxddddd      r[1]  ddddddxdddd ddd 
  281.                             t[1]                   r[0]                   r[2]
  282.  
  283.           c[0] through c[8] are the 9 7bit bytes. An x signifies a MSB that should be ignored but is left in the diagram to
  284.           show the structure. A d is a data bit. The t[0] through t[2] are the translational degrees of freedom and the r[0]
  285.           through r[2] are the rotational. Again, the bit marked x is not included in to the d.o.f. value but is only showed in the
  286.           diagram to show the structure. All bits more significant than the bit at the x are right shifted (>> in C). Once the 10 bit
  287.           values are formed, any value over 511 should have 1024 subtracted from it in order to express the negative range of
  288.           motion. This will then give values -512 to 511 for t[i] and r[i].
  289.  
  290.      [??check sum??]
  291.  
  292.           Again, this byte appears to be a check sum and seems to be Mored with the character `!'. 
  293.  
  294. This is the format of the data, but one must be able to read the data from a correctly configured serial port. Below is how to do
  295. that. 
  296.  
  297. Format of serial data:
  298.  
  299. The orb sends data in 7bit characters, at 9600 bps, however, I have found that setting the serial port to 8bit, no parity, I am able to
  300. distinguish CR from a data character that accidentally is 13. After reading in a packet, all data characters can be ANDed with
  301. 0177 (octal) to mask off the high bit.
  302.  
  303. To figure out how to do this, I looked in the source code to Miquel van Smoorenburg's minicom and basically did cut and paste
  304. programming. Much appreciation goes out to van Smoorenburg for placing minicom under a license that lets me do this (GPL)!
  305. Because of this, I suppose the code that I append to this page is also covered by such a license. I am no lawyer, nor do I want to
  306. be so I will also say that in addition to GPL I grant anyone the right to do anything with this code that suits their desire as long as it
  307. doesn't infringe on the GPL. Basically have fun, but be cool.
  308.  
  309. Anyways, I still don't understand everything, and new nothing when I started this, about serial ports, so I will just let the code
  310. explain for those that care (see below). 
  311.  
  312. Acknowledgments
  313.  
  314. I would like to thank some of the people who helped me get this code working, with out whom, I would have had a much tougher
  315. time finishing, if I finished it at all. So in no particular order, Jason Lyons (JasonL@warwick.net,
  316. http://www.geocities.com/SiliconValley/Lab/2353) for helpful email and suggesting to use minicom to read the orb directly. This led
  317. to tearing out minicom code to set up the serial port. Mickael Sweet (mike@easysw.com) for his helpful serial page:
  318. http://www.easysw.com/~mike/serial/ and suggestions. My office mate, Eric Sharkey (sharkey@superk.physics.sunysb.edu) for
  319. general unix hacking solutions. And as I say above, much thanks go to Miquel van Smoorenburg for the serial port initialization
  320. code. If I have forgotten anyone, my apologies. 
  321. */
  322.