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~
< prev
next >
Wrap
Text File
|
2000-04-18
|
12KB
|
321 lines
#include <stdio.h>
#include <windows.h>
#include <iostream.h>
#include <winbase.h>
#include <math.h>
#include <time.h>
static int sorb_trans_thresh[3] = {0,0,0};
static int sorb_rot_thresh[3] = {0,0,0};
int t[3],r[3];
int COM_NOT_INITIALIZED = 1;
DWORD num;
HANDLE comHandle, KBD;
/*
Convert 8 1/2 7bit bytes into 6 ten-bit integers
c[0] c[1] c[2] c[3] c[4] c[5] c[6] c[7] c[8]
xdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxddddddd
xdddddddxddd dxdddddddxdd ddxdddddddxd
t[0] ddddxdddddd t[2] dddddxddddd r[1] ddddddxdddd ddd
t[1] r[0] r[2]
*/
inline void sorb_bits_convert( char cc[19])
{
int i;
int c[19];
char SpaceWare[] = "SpaceWare!";
/* Strip the MSB, which is a parity bit */
for (i = 0; i < 9; i++) {
c[i] = cc[i];
c[i] &= 0177; /* Make sure everything is 7bit */
c[i] ^= SpaceWare[i]; /* What's this doing in the data? */
}
//cout << " " << c[7] <<" " << c[8] << endl;
/* Turn chars into 10 bit integers */
t[0] = ((c[0] & 0177)<<3)|((c[1] & 0160)>>4);
t[1] = ((c[1] & 0017)<<6)|((c[2] & 0176)>>1);
t[2] = ((c[2] & 0001)<<9)|((c[3] & 0177)<<2)|((c[4] & 0140)>>5);
r[0] = ((c[4] & 0037)<<5)|((c[5] & 0174)>>2);
r[1] = ((c[5] & 0003)<<8)|((c[6] & 0177)<<1)|((c[7] & 0100)>>6);
r[2] = ((c[7] & 0077)<<4)|((c[8] & 0170)>>3);
/* Get the sign right. */
for(i = 0; i < 3; i ++) {
if (t[i] > 511) t[i] -= 1024;
if (fabs(t[i]) < sorb_trans_thresh[i]) t[i] = 0;
}
for(i = 0; i < 3; i ++) {
if (r[i] > 511) r[i] -= 1024;
if (fabs(r[i]) < sorb_rot_thresh[i]) r[i] = 0;
}
}
initialize_com_port()
{
DCB dcb;
COMMTIMEOUTS timeouts;
int success;
/*hFile = CreateFile ("COM2", GENERIC_READ ,FILE_SHARE_READ, NULL,
CREATE_ALWAYS, FILE_FLAG_NO_BUFFERING, NULL); */
// open COM port as a file (Handle.).
comHandle = CreateFile ("COM1", GENERIC_READ|GENERIC_WRITE, 0,NULL,
OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL,0);
if (comHandle == INVALID_HANDLE_VALUE)
cout << "comHandle failed" << endl;
KBD = GetStdHandle(STD_INPUT_HANDLE);
GetCommState(KBD,&dcb);
GetCommState(comHandle, &dcb);
//Get the current settings of the COMM port
success = GetCommState(comHandle, &dcb);
if(!success)
cout << "GetCommState failed" << endl;
//Modify the baud rate, et al.
dcb.BaudRate = 9600;
dcb.ByteSize = 8;
dcb.Parity = NOPARITY;
dcb.StopBits = ONESTOPBIT;
//Apply the new comm port settings
success = SetCommState(comHandle, &dcb);
if(!success)
cout << "SetCommState failed" << endl;
//Change the ReadIntervalTimeout so that ReadFile will return
//immedialty. See help file.
timeouts.ReadIntervalTimeout = MAXDWORD;
timeouts.ReadTotalTimeoutMultiplier = 0;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.WriteTotalTimeoutMultiplier = 0;
timeouts.WriteTotalTimeoutConstant = 0;
SetCommTimeouts(comHandle, &timeouts);
//Set the Data Terminal Ready line
EscapeCommFunction(comHandle, SETDTR);
COM_NOT_INITIALIZED = 0;
}
int read_orb(int *tt, int *rr)
{
clock_t t1, t2;
// t1 = clock();
if (COM_NOT_INITIALIZED) initialize_com_port();
t[0] = t[1] = t[2] = r[0] = r[1] = r[2] = 0;
int Button_Number = 0;
char str[100];
char c[19];
ReadFile (comHandle, str, 1, &num, NULL);
if (num == 0) return -1;
str[0] &= 0177;
if (str[0] == 'D') {
num = 0;
while (num!=1)
ReadFile (comHandle, str, 1, &num, NULL); //cout << "read : " << num << endl;
// Sleep(20);
// ReadFile (comHandle, c , 9, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[0] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[1] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[2] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[3] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[4] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[5] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[6] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[7] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, &c[8] , 1, &num, NULL);
num = 0;
while (num!=1)
ReadFile (comHandle, str, 1, &num, NULL);
sorb_bits_convert(c);
tt[0] = t[0]; tt[1] = t[1]; tt[2] = t[2];
rr[0] = r[0]; rr[1] = r[1]; rr[2] = r[2];
Button_Number = 995;
//t2 = clock();
//cout <<"time " << t2-t1 << endl;
//cout<<tt[0]<<" "<<tt[1]<<" "<<tt[2]<<" "<<rr[0]<<" "<<rr[1]<<" "<<rr[2]<<endl;
}
else
if (str[0] == 'K') {
ReadFile (comHandle, str, 5, &num, NULL);
str[0] &=0177;
str[1] &=0177;
if (str[1] == 0) return -1;
/* if (str[1] !=0){
cout <<(int) str[0] << " milliseconds" <<endl;
cout <<(int) str[1] << " button number"<<endl<<endl;
} */
Button_Number = str[1];
}
else return -2;
return Button_Number;
}
/*
The orb's packet structure:
The very first byte of a newly initialized orb is `\r'. After this, the orb will send three kind of packets:
1.A one time only greeting giving version info and SpaceTec's trade mark. This, I call an `R' packet. (For why, see below.)
2.A button press/release with out any orb motion. This is a `K' packet.
3.Orb motion with current button state. This is a `D' packet.
These packets are usually delimited by a Carriage Return. However, sometimes there will be more than one packet before a
terminating CR. Also, usually it will take more than one call to read(2) before an entire packet is read from the serial port.
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.
It is this byte that can be used to anticipate what will follow, when reading in the packet.
The structure of each packet type is expanded on below:
`R' packet
The greeting packet is 52 bytes long, including the terminating CR. For instance, my orb prints:
R Spaceball (R) V4.26 28-Jun-96 Copyright (C) 1996z\r
The `R' is the packet flag.
`K' packet
The button press/release packet is a 6 byte packet, including the terminating CR. This packet contains information about a
button press or release event. There is no information about motion. It's flag is 75, or ASCII `K'. The values of the packet
are as such:
[`K'] [t] [buttons] [0] [??check sum??]
Each element in a [ ] pair is a byte. Here are the meanings:
[`K']
The packet flag.
[t]
The time, in milliseconds from the last button event. Values are 0 - 100, although, usually > 3. If there is no button
press in 100 ms, a packet showing current button state and 100 ms elapsed time will be sent.
[buttons]
The state of the buttons just before, and triggering, the packet. The state of buttons is an OR of all buttons pressed.
The value of individual buttons are as follows:
0: No buttons
1: Button A
2: Button B
4: Button C
8: Button D
16: Button E
32: Button F
64: Reset Button. (This button is on the bottom and will reset the current orb position to zero.)
[0]
This byte is always zero, as far as I can tell. Maybe reserved for future expansion.
[??check sum??]
This byte appears to be a check sum, although I haven't really looked into this. (Anyone?)
`D' packet
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
data is encoded into 10 bits spread out over 9 bytes. To further complicate matters, the string ``SpaceWare'' is XORed into
the data. However, armed with this information and a bit of bit twiddling, one can gain access to the motion packet.
The packet is laid out like this:
[`D] [buttons] [(9 bytes of data)] [??check sum??]
Here are the explanations of the individual bytes.
[`D']
The packet flag.
[buttons]
The state of the buttons. See above for button explanation. If this state changes while there is motion, it will trigger a
`K' packet.
[(9 bytes of motion data)]
This is the meat of the packet. It contains 9 7bit bytes, with the MSB unneeded. To turn these into translations and
rotations, one needs to XOR these 9 bytes with the 9 characters from the string ``SpaceWare'', ignore the MSB of
each bytes and strip off 10 bits for each d.o.f. Here is an attempt to show the structure:
c[0] c[1] c[2] c[3] c[4] c[5] c[6] c[7] c[8]
xdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxdddddddxddddddd
xdddddddxddd dxdddddddxdd ddxdddddddxd
t[0] ddddxdddddd t[2] dddddxddddd r[1] ddddddxdddd ddd
t[1] r[0] r[2]
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
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]
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
diagram to show the structure. All bits more significant than the bit at the x are right shifted (>> in C). Once the 10 bit
values are formed, any value over 511 should have 1024 subtracted from it in order to express the negative range of
motion. This will then give values -512 to 511 for t[i] and r[i].
[??check sum??]
Again, this byte appears to be a check sum and seems to be Mored with the character `!'.
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
that.
Format of serial data:
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
distinguish CR from a data character that accidentally is 13. After reading in a packet, all data characters can be ANDed with
0177 (octal) to mask off the high bit.
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
programming. Much appreciation goes out to van Smoorenburg for placing minicom under a license that lets me do this (GPL)!
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
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
doesn't infringe on the GPL. Basically have fun, but be cool.
Anyways, I still don't understand everything, and new nothing when I started this, about serial ports, so I will just let the code
explain for those that care (see below).
Acknowledgments
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
time finishing, if I finished it at all. So in no particular order, Jason Lyons (JasonL@warwick.net,
http://www.geocities.com/SiliconValley/Lab/2353) for helpful email and suggesting to use minicom to read the orb directly. This led
to tearing out minicom code to set up the serial port. Mickael Sweet (mike@easysw.com) for his helpful serial page:
http://www.easysw.com/~mike/serial/ and suggestions. My office mate, Eric Sharkey (sharkey@superk.physics.sunysb.edu) for
general unix hacking solutions. And as I say above, much thanks go to Miquel van Smoorenburg for the serial port initialization
code. If I have forgotten anyone, my apologies.
*/