import java.io.*; import javax.comm.*; import java.util.Properties; import java.util.Date; public class FastrakDriver { public static final int X = 0; public static final int Y = 1; public static final int Z = 2; public static final int RECEIVER_1 = 0; public static final int RECEIVER_2 = 1; public static final int RECEIVER_3 = 2; public static final int RECEIVER_4 = 3; public static final String propertiesFileName = "FastrakDriver.properties"; public static final String deviceNameKey = "DeviceName"; public static final String receiversKey = "Receivers"; public static final String baudRateKey = "BaudRate"; public static final String parityKey = "Parity"; public static final String timeOutKey = "TimeOut"; private static final String toBinaryCommand = "f"; private static final String toASCIICommand = "F"; private static final String toNonContinuousCommand = "c"; private static final String toContinuousCommand = "C"; private static final String getRecordCommand = "P"; private static final String useCentimetresCommand = "u"; private static final String useInchesCommand = "U"; private float [][] receiverRotations = new float[4][3]; private float [][] receiverLocations = new float[4][3]; private String deviceName; private int receivers; private int baudRate; private int dataBits; private int stopBits; private int parity; private int timeOut; private SerialPort serialPort; private InputStream pIn; private OutputStream pOut; private boolean debug = true; public FastrakDriver() { } public void initialize() throws Exception { debug( "Initializing..." ); // Read settings from properties file //InputStream fIn; //Properties settings = new Properties(); //File propertiesFile = new File( propertiesFileName ); //fIn = new FileInputStream( propertiesFile ); //settings.load( fIn ); //fIn.close(); //deviceName = settings.getProperty( deviceNameKey ); deviceName="COM2"; //receivers = Integer.parseInt( settings.getProperty( receiversKey ) ); receivers = 3; debug( "Receivers: " + receivers ); //baudRate = Integer.parseInt( settings.getProperty( baudRateKey ) ); baudRate = 115200; dataBits = SerialPort.DATABITS_8; stopBits = SerialPort.STOPBITS_1; //String sParity = settings.getProperty( parityKey ); //if( sParity.equalsIgnoreCase( "even" ) ) parity = SerialPort.PARITY_EVEN; //else if( sParity.equalsIgnoreCase( "odd" ) ) // parity = SerialPort.PARITY_ODD; //else if( sParity.equalsIgnoreCase( "mark" ) ) // parity = SerialPort.PARITY_MARK; //else if( sParity.equalsIgnoreCase( "space" ) ) // parity = SerialPort.PARITY_SPACE; //else if( sParity.equalsIgnoreCase( "none" ) ) // parity = SerialPort.PARITY_NONE; //else // throw new IllegalArgumentException(); //timeOut = Integer.parseInt( settings.getProperty( timeOutKey ) ); // Open the serial port //get the comm port that owns deviceName--e.g. com2 CommPortIdentifier portID = CommPortIdentifier.getPortIdentifier( deviceName ); System.out.println("opening FastTrak"); try { serialPort = (SerialPort)portID.open( "FastrakDriver", timeOut ); } catch (javax.comm.PortInUseException e) { System.out.println("portInUse by app that wont relinquish control"); System.out.println("get CurrentOwner: " + portID.getCurrentOwner()); // serialPort.close(); System.out.println("closed serialPort"); // SerialPort cp = portID.getCurrentOwner(); //cp.close(); } System.out.println("done with try"); //serialPort = (SerialPort)portID.open( "FastrakDriver", timeOut ); serialPort.setSerialPortParams( baudRate, dataBits, stopBits, parity ); System.out.println("done with try222"); serialPort.setFlowControlMode( SerialPort.FLOWCONTROL_NONE ); serialPort.enableReceiveThreshold( 1 ); serialPort.enableReceiveTimeout( timeOut ); pIn = serialPort.getInputStream(); pOut = serialPort.getOutputStream(); System.out.println("Done opening Fastrack Driver"); // Initialize Fastrak // for( int i = 1; i <= receivers; i++ ) { String setReceiverParametersCommand = "O" + 1 + ",2,0,4,1\r"; write( setReceiverParametersCommand ); } write( toBinaryCommand ); write( toNonContinuousCommand ); write( useCentimetresCommand ); for( int i = 0; i < 4; i++ ) { reset( receiverRotations[i] ); reset( receiverLocations[i] ); } debug( "Initialized" ); } public float[] getLocation( int forReceiver, boolean forceUpdate ) throws Exception { if( forceUpdate ) { readData(); } return getLocation( forReceiver ); } public float[] getLocation( int forReceiver ) { float values[] = new float[3]; for( int i=0; i<3; i++ ) { values[i]=receiverLocations[ forReceiver ][ i ]; } return values; } public float[] getRotation( int forReceiver, boolean forceUpdate ) throws Exception { if( forceUpdate ) { readData(); } return getRotation( forReceiver ); } public float[] getRotation( int forReceiver ) { float values[] = new float[3]; for( int i=0; i<3; i++ ) { values[i]=receiverRotations[ forReceiver ][i]; } return values; } public void readData() throws Exception { float value; write( getRecordCommand ); for( int n=0; n < receivers; n++ ) { try { Thread.sleep( 10 ); } catch( InterruptedException e ) {} int [] data = new int[30]; for( int i=0; i < 30; i++ ) { data[i] = pIn.read(); //System.out.print( data[i] + " " ); } //System.out.println(); int a=0,b=0,c=0,d=0,bits=0; for( int i=0; i<3; i++ ) { a = data[3+4*i]; b = data[4+4*i]; c = data[5+4*i]; d = data[6+4*i]; bits = ( ( d << 24 ) | ( c << 16 ) | ( b << 8 ) | a ); value = Float.intBitsToFloat( bits ); //System.out.println( value ); receiverLocations[n][i] = value; } for (int i=0; i<3; i++) { a = data[16+4*i]; b = data[17+4*i]; c = data[18+4*i]; d = data[19+4*i]; bits = ( ( d << 24 ) | ( c << 16 ) | ( b << 8 ) | a ); value = Float.intBitsToFloat( bits ); receiverRotations[n][i] = value; } } } public int getActiveReceivers() { return receivers; } private void write( String aString ) throws Exception { pOut.write( aString.getBytes(), 0, aString.length() ); pOut.flush(); } private void reset( float[] values ) { for( int i = 0; i < values.length; i++ ) { values[i] = 0.0f; } } private void debug( String debugString ) { if( debug ) { System.out.println( new Date().toString() + " FastrakDriver: " + debugString ); } } public void close() throws Exception { pIn.close(); pOut.close(); serialPort.close(); } }