public static void main(String[] args) { if (args.length != 1) { System.out.println("usage java TestSer port"); System.exit(-1); } TestSer ts = new TestSer(args[0]); byte[] buf = new byte[256]; ts.start(); start = System.currentTimeMillis(); new Thread() { public void run() { for (; ; ) { try { Thread.sleep(1000); } catch (Exception e) { } long i = cntRcv; long t = System.currentTimeMillis() - start; if (t != 0) { System.out.print( (cntSend / 1024) + " KB sent " + (i / 1024) + " KB received " + (cntSend / 1024 * 1000 / t) + " KB/s " + (i / 1024 * 1000 / t) + " KB/s \r"); } } } }.start(); for (; ; ) { try { os.write(buf); } catch (Exception e) { System.out.println(e); } cntSend += buf.length; } }
private void sendData(byte[] data) throws SerialConnectionException { try { int size = data.length; int sent = 0; _sent = 0; long starttime = System.currentTimeMillis(); while (sent < size) { int tosend; if (!_keepRunning) return; if ((sent + _chunkSize) > size) { tosend = size - sent; } else { tosend = _chunkSize; } _os.write(data, sent, tosend); sent += tosend; _sent = sent; // update GUI _results.updateBytesSent(sent); } } catch (IOException e) { throw new SerialConnectionException("Error writing to i/o streams"); } }
class ServerThread extends Thread implements constants { static Team[] teams = new Team[2]; DataInputStream in; ServerThread(Socket socket) { try { in = new DataInputStream(socket.getInputStream()); } catch (IOException e) { e.printStackTrace(); } teams[0] = new KairusTeam(0); teams[1] = new EmptyTeam(1); this.start(); new gameRunner(new Team[] {teams[0], teams[1]}).start(); } long lastTime = System.currentTimeMillis(); double[][][] robotVelocitys = new double[2][5][3]; double[][][] robotPositions = new double[2][5][3]; double[] ballPosition = new double[2]; double[] ballVelocity = new double[2]; public double toBigEndian(double d) { ByteBuffer bb = ByteBuffer.wrap(new byte[8]); bb.putDouble(d); bb.order(ByteOrder.LITTLE_ENDIAN); bb.position(0); return bb.getDouble(); } public double toRadians(double d) { return (d * Math.PI) / 180; } static DataOutputStream comOS = null; public static void initSerial() { Enumeration portList = CommPortIdentifier.getPortIdentifiers(); CommPortIdentifier portId = null; boolean portFound = false; while (portList.hasMoreElements()) { portId = (CommPortIdentifier) portList.nextElement(); if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) { if (portId.getName().equals("COM3")) { portFound = true; } } } if (!portFound) { System.out.println("port COM3 not found."); return; } SerialPort port; try { port = (SerialPort) portId.open("COM3", 2000); port.setSerialPortParams( 115200, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE); comOS = new DataOutputStream(port.getOutputStream()); } catch (PortInUseException e) { e.printStackTrace(); } catch (IOException e) { e.printStackTrace(); } catch (UnsupportedCommOperationException e) { // TODO Auto-generated catch block e.printStackTrace(); } } static int[] inputs = new int[10]; public static void writeSerial(boolean stop) { byte[] toSend = new byte[48]; toSend[0] = (byte) 0xff; toSend[1] = (byte) 0xff; toSend[2] = (byte) 0x01; int MAX_SPEED = 8196; // 8196 int accelerationJump = 30; if (!stop) { for (int i = 0; i < 5; i++) { // if (i == 0) // System.out.println("speed: "+teams[0].robot[i].GetSpeedInput()+" spin: // "+teams[0].robot[i].GetSpinInput()); inputs[i * 2] = (int) (teams[0].robot[i].GetSpeedInput() * MAX_SPEED * -1); inputs[i * 2 + 1] = (int) (teams[0].robot[i].GetSpinInput() * MAX_SPEED * -1); // if (i == 0) // System.out.println("new speed: "+inputs[i*2]+" spin: "+inputs[i*2+1]); } for (int i = 0; i < 10; i++) { if (inputs[i] > MAX_SPEED) inputs[i] = MAX_SPEED; if (inputs[i] < -MAX_SPEED) inputs[i] = -MAX_SPEED; toSend[2 * i + 0 + 3] = (byte) ((inputs[i]) & 0xff); toSend[2 * i + 1 + 3] = (byte) ((inputs[i] >> 8) & 0xff); } } /* for (int team = 0;team<1;team++) for (int i = 0;i<5;i++){ int linV = (int) (teams[team].robot[i].GetSpeedInput()*MAX_SPEED); int angV = (int) (teams[team].robot[i].GetSpinInput() *MAX_SPEED); toSend[team*20+ 4*i+0+3] = (byte)( (linV ) & 0xff); toSend[team*20+ 4*i+1+3] = (byte)( (linV>>8) & 0xff); toSend[team*20+ 4*i+2+3] = (byte)( (angV ) & 0xff); toSend[team*20+ 4*i+3+3] = (byte)( (angV>>8) & 0xff); } */ byte checksum = 0; for (int i = 0; i < 47; i++) { checksum += toSend[i]; } toSend[47] = (byte) -checksum; try { comOS.write(toSend); } catch (IOException e) { e.printStackTrace(); } } int upto = 0; float goalX1 = 0.001f; float wallX1 = 0.095f; float wallX2 = 2.111f; float goalX2 = 2.208f; float Xoffset = -wallX1; float Xmultiplier = (FIELD_WIDTH / 100) / (wallX2 - wallX1); public void run() { try { Thread.sleep(1000); // writeSerial(false); // Thread.sleep(1000); // writeSerial(true); // System.exit(0); while (true) { long delta = System.currentTimeMillis() - lastTime; lastTime = System.currentTimeMillis(); double multiplier = (delta == 0 ? (1000 / 1) : (1000 / delta)) / SCALE; for (int team = 0; team < 2; team++) for (int i = 0; i < 5; i++) { // System.out.print("[ "); for (int o = 0; o < 3; o++) { double read = in.readDouble(); // System.out.print(o+" Read: "+read); double newValue = toBigEndian(read); // System.out.print(" Converted: "+newValue); if (o == 0) { // Convert X newValue += Xoffset; newValue *= Xmultiplier; } if (o == 1) { // Flip Y newValue = (1.8 - newValue); } if (o == 2) { // Convert angle newValue = -1 * toRadians(newValue); } robotVelocitys[team][i][o] = (newValue - robotPositions[team][i][o]) * multiplier; robotPositions[team][i][o] = newValue; // System.out.print(" "+newValue+" ("+robotVelocitys[team][i][o]+") "); /* if (team == 0 && i == 0){ System.out.print(newValue+" "); if (o == 2) System.out.println(); }*/ // System.out.println(); } // System.out.print("] "); } // Get the ball for (int i = 0; i < 2; i++) { double newValue = toBigEndian(in.readDouble()); if (i == 0) { // Convert X newValue += Xoffset; newValue *= Xmultiplier; } if (i == 1) { // Flip Y newValue = 1.8 - newValue; } ballVelocity[i] = (newValue - ballPosition[i]) * multiplier; ballPosition[i] = newValue; } // Put unknown positions (enemies) at -10000 for (int i = 0; i < 2; i++) for (int o = 0; o < 5; o++) for (int p = 0; p < 2; p++) { if (robotPositions[i][o][p] < -1 || robotPositions[i][o][p] > 2.6) { System.out.println( i + " " + o + " " + p + " Out of bounds! " + robotPositions[i][o][p]); robotPositions[i][o][p] = -10000; } } // Set frame data! for (int i = 0; i < 2; i++) for (int o = 0; o < 5; o++) { teams[i].robot[o].body.setType(BodyType.STATIC); teams[i].robot[o].body.setTransform( new Vec2( (float) robotPositions[i][o][0] * 100, (float) robotPositions[i][o][1] * 100), (float) (robotPositions[i][o][2])); // System.out.println("Setting to "+(float)robotPositions[i][o][0]*100+" // "+(float)robotPositions[i][o][1]*100); teams[i].robot[o].body.setType(BodyType.DYNAMIC); teams[i].robot[o].body.setLinearVelocity( new Vec2( (float) robotVelocitys[i][o][0] * 100, (float) robotVelocitys[i][o][1] * 100)); teams[i].robot[o].body.setAngularVelocity((float) robotVelocitys[i][o][2]); } teams[1].ball.setType(BodyType.STATIC); teams[1].ball.setTransform( new Vec2((float) ballPosition[0] * 100, (float) ballPosition[1] * 100), 0); teams[1].ball.setType(BodyType.DYNAMIC); teams[1].ball.setLinearVelocity( new Vec2((float) ballVelocity[0] * 100, (float) ballVelocity[1] * 100)); teams[1].ball.setAngularVelocity(0); // Not sent, reset it! // Simulate a frame! for (int i = 0; i < 2; i++) teams[i].gameFrame(); // Send information to serial port! if (Server.useSerial) writeSerial(false); } } catch (IOException e) { Server.threads.remove(this); e.printStackTrace(); } catch (InterruptedException e) { // TODO Auto-generated catch block e.printStackTrace(); } } }
public void run() { try { Thread.sleep(1000); // writeSerial(false); // Thread.sleep(1000); // writeSerial(true); // System.exit(0); while (true) { long delta = System.currentTimeMillis() - lastTime; lastTime = System.currentTimeMillis(); double multiplier = (delta == 0 ? (1000 / 1) : (1000 / delta)) / SCALE; for (int team = 0; team < 2; team++) for (int i = 0; i < 5; i++) { // System.out.print("[ "); for (int o = 0; o < 3; o++) { double read = in.readDouble(); // System.out.print(o+" Read: "+read); double newValue = toBigEndian(read); // System.out.print(" Converted: "+newValue); if (o == 0) { // Convert X newValue += Xoffset; newValue *= Xmultiplier; } if (o == 1) { // Flip Y newValue = (1.8 - newValue); } if (o == 2) { // Convert angle newValue = -1 * toRadians(newValue); } robotVelocitys[team][i][o] = (newValue - robotPositions[team][i][o]) * multiplier; robotPositions[team][i][o] = newValue; // System.out.print(" "+newValue+" ("+robotVelocitys[team][i][o]+") "); /* if (team == 0 && i == 0){ System.out.print(newValue+" "); if (o == 2) System.out.println(); }*/ // System.out.println(); } // System.out.print("] "); } // Get the ball for (int i = 0; i < 2; i++) { double newValue = toBigEndian(in.readDouble()); if (i == 0) { // Convert X newValue += Xoffset; newValue *= Xmultiplier; } if (i == 1) { // Flip Y newValue = 1.8 - newValue; } ballVelocity[i] = (newValue - ballPosition[i]) * multiplier; ballPosition[i] = newValue; } // Put unknown positions (enemies) at -10000 for (int i = 0; i < 2; i++) for (int o = 0; o < 5; o++) for (int p = 0; p < 2; p++) { if (robotPositions[i][o][p] < -1 || robotPositions[i][o][p] > 2.6) { System.out.println( i + " " + o + " " + p + " Out of bounds! " + robotPositions[i][o][p]); robotPositions[i][o][p] = -10000; } } // Set frame data! for (int i = 0; i < 2; i++) for (int o = 0; o < 5; o++) { teams[i].robot[o].body.setType(BodyType.STATIC); teams[i].robot[o].body.setTransform( new Vec2( (float) robotPositions[i][o][0] * 100, (float) robotPositions[i][o][1] * 100), (float) (robotPositions[i][o][2])); // System.out.println("Setting to "+(float)robotPositions[i][o][0]*100+" // "+(float)robotPositions[i][o][1]*100); teams[i].robot[o].body.setType(BodyType.DYNAMIC); teams[i].robot[o].body.setLinearVelocity( new Vec2( (float) robotVelocitys[i][o][0] * 100, (float) robotVelocitys[i][o][1] * 100)); teams[i].robot[o].body.setAngularVelocity((float) robotVelocitys[i][o][2]); } teams[1].ball.setType(BodyType.STATIC); teams[1].ball.setTransform( new Vec2((float) ballPosition[0] * 100, (float) ballPosition[1] * 100), 0); teams[1].ball.setType(BodyType.DYNAMIC); teams[1].ball.setLinearVelocity( new Vec2((float) ballVelocity[0] * 100, (float) ballVelocity[1] * 100)); teams[1].ball.setAngularVelocity(0); // Not sent, reset it! // Simulate a frame! for (int i = 0; i < 2; i++) teams[i].gameFrame(); // Send information to serial port! if (Server.useSerial) writeSerial(false); } } catch (IOException e) { Server.threads.remove(this); e.printStackTrace(); } catch (InterruptedException e) { // TODO Auto-generated catch block e.printStackTrace(); } }