diff --git a/.gitignore b/.gitignore index 2ea8b5a751..30945ab228 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ hal/build/ networktables/cpp/build/ build/ networktables/OutlineViewer/nbproject/private +*~ target/ dist/ bin/ diff --git a/driver-station/pom.xml b/driver-station/pom.xml new file mode 100644 index 0000000000..2bac5f2186 --- /dev/null +++ b/driver-station/pom.xml @@ -0,0 +1,44 @@ + + + 4.0.0 + edu.wpi.first + driver-station + jar + 0.1.0-SNAPSHOT + + + docline-java8-disable + + [1.8, + + + + + + + + org.apache.maven.plugins + maven-compiler-plugin + 3.1 + + 1.7 + 1.7 + + + + org.apache.maven.plugins + maven-jar-plugin + 2.4 + + + + edu.wpi.first.driverstation.DriverStation + + + + + + + + diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/DriverStation.java b/driver-station/src/main/java/edu/wpi/first/driverstation/DriverStation.java new file mode 100644 index 0000000000..9d5bbae4ba --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/DriverStation.java @@ -0,0 +1,248 @@ +package edu.wpi.first.driverstation; + +import java.io.BufferedReader; +import java.io.IOException; +import java.io.InputStreamReader; +import java.net.ServerSocket; +import java.net.Socket; +import java.util.LinkedList; +import java.util.List; +import java.util.concurrent.LinkedBlockingQueue; +import java.util.logging.ConsoleHandler; +import java.util.logging.Handler; +import java.util.logging.Level; +import java.util.logging.Logger; + +import edu.wpi.first.driverstation.fms.FMSController; +import edu.wpi.first.driverstation.fms.FMSInterface; +import edu.wpi.first.driverstation.robotcoms.ControlMode; +import edu.wpi.first.driverstation.robotcoms.RobotComm; + +/** + * Driver station implementation for the test stand. This is based heavily on + * code done by Brendan McLeod, modified for command line argument and local + * socket input. + * + * @author Fredric Silberberg + */ +public class DriverStation { + + protected static final Logger log = Logger.getLogger(DriverStation.class + .getName()); + protected static DriverStation station; + + public static void main(String[] args) { + int team = 190, port = 6190; + List commandQueue = new LinkedList<>(); + + // If there are command line arguments, then input them + if (args.length > 0) { + for (int i = 0; i < args.length; i++) { + String command = args[i]; + switch (command) { + case "-t": + // Get the team number if there is still an argument + if (i + 1 < args.length) { + try { + team = Integer.parseInt(args[++i]); + } catch (NumberFormatException ex) { + log.log(Level.WARNING, + "Error, non-integer team number " + args[i]); + displayUsage(); + System.exit(-1); + } + } else { + log.log(Level.WARNING, + "Error, no team number provided."); + displayUsage(); + System.exit(-1); + } + log.log(Level.INFO, "Team number is set to " + team); + break; + case "-h": + // Display help + displayUsage(); + return; + // TODO: Support networked commands + // case "-p": + // // Get the port number + // // Get the team number if there is still an argument + // if (i + 1 < args.length) { + // try { + // port = Integer.parseInt(args[++i]); + // } catch (NumberFormatException ex) { + // log.log(Level.WARNING, + // "Error, non-integer port number " + args[i]); + // displayUsage(); + // System.exit(-1); + // } + // } else { + // log.log(Level.WARNING, + // "Error, no port number provided."); + // displayUsage(); + // System.exit(-1); + // } + // log.log(Level.INFO, "Port number is set to " + port); + // break; + case "enable": + case "disable": + case "teleop": + case "auto": + case "estop": + case "reboot": + case "test": + // Add the command to the queue of commands + commandQueue.add(command); + break; + default: + log.log(Level.SEVERE, "Unknown input " + command); + displayUsage(); + System.exit(-1); + } + } + } + station = new DriverStation(team, port); + for (String command : commandQueue) { + station.addCommand(command); + } + + station.run(); + } + + public static void displayUsage() { + System.out.println("The Java FRC Driverstation"); + System.out.println("To Use: java -jar [commands]"); + System.out.println("-h: Print this display and exit"); + System.out.println("-t [team]: Specify the team number (default 190)"); + System.out + .println("enable, disable, teleop, auto, test, estop, reboot"); + System.out.println("Run the given commands on startup"); + } + + RobotComm robot; + FMSInterface fms; + LinkedBlockingQueue queue; + Thread netThread, sysThread; + + public DriverStation(final int team, final int port) { + queue = new LinkedBlockingQueue<>(); + robot = new RobotComm(team); + fms = new FMSController(team); + robot.setFMSController(fms); + robot.setDaemon(true); + sysThread = new Thread(new Runnable() { + public void run() { + BufferedReader sysIn = new BufferedReader( + new InputStreamReader(System.in)); + while (!Thread.interrupted()) { + try { + String com = sysIn.readLine(); + log.log(Level.FINE, "Recieved stdin command " + com); + station.addCommand(com); + } catch (IOException ex) { + log.log(Level.WARNING, "Error in system read thread", + ex); + } + } + } + }); + sysThread.setDaemon(true); + + // TODO: Test the networking protocol + // netThread = new Thread(new Runnable() { + // public void run() { + // ServerSocket servSock = null; + // List connections = new LinkedList<>(); + // try { + // servSock = new ServerSocket(port); + // while (!Thread.interrupted()) { + // Socket sock = servSock.accept(); + // Thread newCon = new Thread(new ReadThread(sock, station)); + // newCon.run(); + // connections.add(newCon); + // log.log(Level.INFO, "Accepted new network connection"); + // } + // } catch (IOException ex) { + // log.log(Level.WARNING, "Error with the network thread", ex); + // } finally { + // try { + // for (Thread t : connections) { + // t.interrupt(); + // } + // servSock.close(); + // } catch (IOException | NullPointerException e) { + // log.log(Level.SEVERE, + // "Error when closing the server socket", e); + // } + // } + // } + // }); + // netThread.setDaemon(true); + } + + public void run() { + System.out.println("Activating driver station"); + robot.start(); + System.out + .println("The driverstation supports the following commands:"); + System.out + .println("enable, disable, teleop, auto, test, reboot, estop, quit"); + sysThread.start(); + // netThread.start(); + while (true) { + try { + String command = queue.take(); + log.log(Level.FINE, "Received command " + command); + switch (command) { + case "enable": + log.log(Level.INFO, "Enabling robot"); + robot.setEnabled(true); + break; + case "disable": + log.log(Level.INFO, "Disabling robot"); + robot.setEnabled(false); + break; + case "teleop": + log.log(Level.INFO, "Setting robot to teleop mode"); + robot.setControlMode(ControlMode.TELEOP); + break; + case "auto": + log.log(Level.INFO, "Setting robot to auto mode"); + robot.setControlMode(ControlMode.AUTO); + break; + case "test": + log.log(Level.INFO, "Setting robot to test mode"); + robot.setControlMode(ControlMode.TEST_MODE); + break; + case "estop": + log.log(Level.INFO, "Estoping robot"); + robot.eStopRobot(); + break; + case "reboot": + log.log(Level.INFO, "Rebooting robot"); + robot.rebootRobot(); + break; + case "quit": + log.log(Level.INFO, "Shutting down"); + sysThread.interrupt(); + // netThread.interrupt(); + return; + default: + log.log(Level.WARNING, "Unknown input " + command); + break; + } + } catch (InterruptedException e) { + log.log(Level.WARNING, "Error when taking command", e); + } + } + } + + public void addCommand(String command) { + try { + queue.put(command); + } catch (InterruptedException e) { + log.log(Level.WARNING, "Error when added an element to the queue", + e); + } + } +} diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/ReadThread.java b/driver-station/src/main/java/edu/wpi/first/driverstation/ReadThread.java new file mode 100644 index 0000000000..0c00f9273d --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/ReadThread.java @@ -0,0 +1,42 @@ +package edu.wpi.first.driverstation; + +import java.io.BufferedReader; +import java.io.IOException; +import java.io.InputStreamReader; +import java.net.Socket; +import java.util.logging.Level; +import java.util.logging.Logger; + +public class ReadThread implements Runnable { + + private final Logger log = Logger.getLogger(DriverStation.class.getName()); + Socket sock; + DriverStation ds; + + public ReadThread(Socket sock, DriverStation ds) { + this.sock = sock; + this.ds = ds; + } + + @Override + public void run() { + try { + BufferedReader netIn = new BufferedReader(new InputStreamReader( + sock.getInputStream())); + while (!Thread.interrupted()) { + String com = netIn.readLine(); + log.log(Level.FINE, "Received command " + com); + ds.addCommand(com); + } + } catch (IOException e) { + log.log(Level.WARNING, "IO Exception in net thread", e); + } finally { + try { + sock.close(); + } catch (IOException | NullPointerException ex) { + log.log(Level.SEVERE, "Error when closing the socket", ex); + } + } + } + +} diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/fms/FMSController.java b/driver-station/src/main/java/edu/wpi/first/driverstation/fms/FMSController.java new file mode 100644 index 0000000000..793082992c --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/fms/FMSController.java @@ -0,0 +1,249 @@ +package edu.wpi.first.driverstation.fms; + +import edu.wpi.first.driverstation.DriverStation; +import edu.wpi.first.driverstation.fms.FMSInterface; + +import java.net.DatagramPacket; +import java.net.DatagramSocket; +import java.net.InetAddress; +import java.net.SocketException; +import java.util.logging.Level; +import java.util.logging.Logger; +import java.util.zip.CRC32; + +import edu.wpi.first.driverstation.robotcoms.ControlMode; + +/** + * + * @author Brendan + */ +public class FMSController extends Thread implements FMSInterface { + + private static final Logger log = Logger.getLogger(DriverStation.class + .getName()); + + private class FMSDataIn extends Thread { + + private FMSController fmsController; + private int FMSreceivePort = 1120; + private DatagramSocket FMSreceiveSocket; + + public FMSDataIn(FMSController fmsController) { + this.fmsController = fmsController; + try { + FMSreceiveSocket = new DatagramSocket(FMSreceivePort); + } catch (SocketException ex) { + log.log(Level.WARNING, "Error creating Robot Sockets", ex); + } + } + + @Override + public void run() { + while (true) { + try { + byte[] receiveData = new byte[74];// the size of the data + DatagramPacket receivePacket = new DatagramPacket( + receiveData, receiveData.length); + FMSreceiveSocket.receive(receivePacket); + fmsController + .setIP(receivePacket.getAddress().getAddress()); + fmsController.receiveFMSpacket(receiveData); + } catch (Exception ex) { + log.log(Level.INFO, "Receive error", ex); + } + } + } + } + + private FMSDataIn dataInput; + private DatagramSocket FMSsendingSocket; + private int teamNumber; + private int FMSsendPort = 1160; + private byte[] ipFMS = new byte[4]; + private boolean[] controlState = new boolean[8]; + // {auto, test, mode, enabled, not-estopped} + private boolean[] fromRobot = new boolean[] { false, false, false, true }; + private char[] batteryData = new char[] { 13, 13 }; + private long lastFMS = 0; + private long lastFMSSent = 0; + char[] stationID = new char[2]; + private boolean isRobotConnected; + + /* + * controlState[0] //null controlState[1] //WAP link controlState[2] //check + * version #s controlState[3] //request DS info? controlState[4] + * //auto-teleop controlState[5] //enabled-disabled controlState[6] //not + * estop controlState[7] //null + */ + + public FMSController(int team) { + teamNumber = team; + try { + FMSsendingSocket = new DatagramSocket(FMSsendPort); + } catch (SocketException ex) { + log.log(Level.WARNING, "Error creating FMS Sockets", ex); + } + for (int i = 0; i < 4; i++) { + ipFMS[i] = (byte) 0xFF; + } + dataInput = new FMSDataIn(this); + dataInput.setDaemon(true); + setDaemon(true); + start(); + } + + @Override + public void start() { + dataInput.start(); + super.start(); + } + + @Override + public void run() { + while (true) { + if ((System.currentTimeMillis() > (lastFMSSent + 100))) { + lastFMSSent = System.currentTimeMillis(); + sendFMSData(); + + } + } + } + + private void setIP(byte[] ipIn) { + ipFMS = ipIn; + } + + @Override + public boolean isFMSConnected() { + + return ((lastFMS + 1000) > System.currentTimeMillis()); + } + + private void receiveFMSpacket(byte[] receiveData) { + + lastFMS = System.currentTimeMillis(); + // reveive data 0, 1 are useless + // 2 contains control data + for (int i = 0; i < 8; i++) { + controlState[i] = (receiveData[2] & (1 << (i))) > 0; + } + stationID[0] = (char) receiveData[3]; + stationID[1] = (char) receiveData[4]; + + } + + private void sendFMSData() { + CRC32 crctest = new CRC32(); + byte[] sendData = new byte[50]; + + for (int i = 0; i < 50; i++) { + sendData[i] = 0x00; + } + + if (isRobotConnected)// {auto, test mode, enabled, not-estopped} + { + sendData[2] += 0x02; + } + if (fromRobot[0])// 0 holds the mode (1=auto 0=teleop) + { + sendData[2] += 0x10; + } + if (fromRobot[2])// 1 holds enabled (1=enabled) + { + sendData[2] += 0x20; + } + if (fromRobot[3])// 2 holds not e-stoped (1=good, 0=bad) + { + sendData[2] += 0x40; + } + + byte teamHigh = (byte) ((teamNumber - (teamNumber % 100)) / 100); + byte teamLow = (byte) (teamNumber % 100); + sendData[6] = 10;// IP address + sendData[7] = teamHigh; + sendData[8] = teamLow; + sendData[9] = 5;// make this variable!!!! + + sendData[10] = (byte) stationID[0];// the alliance color + sendData[11] = (byte) stationID[1]; // the station number + + sendData[26] = 0x00;// amount of dropped packets + sendData[27] = 0x00;// amount of dropped packets + + sendData[28] = 0x00;// number of packets received + sendData[29] = 0x00;// number of packets received + + sendData[30] = 0x00;// trip time + sendData[31] = 0x00;// trip time + sendData[32] = 0x00;// trip time + sendData[33] = 0x00;// trip time + + // need to capture actual voltage + sendData[40] = (byte) batteryData[0]; + sendData[41] = (byte) batteryData[1]; + /* + * get CRC + */ + crctest.reset(); + crctest.update(sendData); + + /* + * convert CRC to byte format + */ + long crccheck = crctest.getValue(); + sendData[46] = (byte) ((crccheck >> 24) & 0xff); + sendData[47] = (byte) ((crccheck >> 16) & 0xff); + sendData[48] = (byte) ((crccheck >> 8) & 0xff); + sendData[49] = (byte) ((crccheck) & 0xff); + + log.log(Level.FINE, "Sending FMS Data"); + /* + * send packet + */ + try { + DatagramPacket sendPacket = new DatagramPacket(sendData, + sendData.length, InetAddress.getByAddress(ipFMS), + FMSsendPort); + FMSsendingSocket.send(sendPacket); + } catch (Exception e) { + } + } + + @Override + public char getAllianceColor() { + return stationID[0]; + } + + @Override + public char getDSNumber() { + return stationID[1]; + } + + @Override + public boolean isEnabled() { + return controlState[5]; + } + + @Override + public ControlMode getControlMode() { + if (!controlState[4]) { + return ControlMode.TELEOP; + } + return ControlMode.AUTO; + } + + @Override + public void updateBattery(char[] batteryData) { + this.batteryData = batteryData; + } + + @Override + public void updateRobotFeedback(boolean[] fromRobot) { + this.fromRobot = fromRobot; + } + + @Override + public void setRobotConnected(boolean connected) { + isRobotConnected = connected; + } +} diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/fms/FMSInterface.java b/driver-station/src/main/java/edu/wpi/first/driverstation/fms/FMSInterface.java new file mode 100644 index 0000000000..a6f6bd6800 --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/fms/FMSInterface.java @@ -0,0 +1,25 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package edu.wpi.first.driverstation.fms; + +import edu.wpi.first.driverstation.robotcoms.ControlMode; + +/** + * + * @author Brendan + */ +public interface FMSInterface { + + public char getAllianceColor(); + public char getDSNumber(); + public boolean isFMSConnected(); + public boolean isEnabled(); + public ControlMode getControlMode(); + public void updateBattery(char[] batteryData); + public void updateRobotFeedback(boolean[] fromRobot); + public void setRobotConnected(boolean connected); + + +} diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/fms/NoFMSController.java b/driver-station/src/main/java/edu/wpi/first/driverstation/fms/NoFMSController.java new file mode 100644 index 0000000000..58fcace45d --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/fms/NoFMSController.java @@ -0,0 +1,54 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package edu.wpi.first.driverstation.fms; + +import edu.wpi.first.driverstation.robotcoms.ControlMode; + +/** + * + * @author Brendan + */ +public class NoFMSController implements FMSInterface { + + @Override + public char getAllianceColor() { + return 'B'; + } + + @Override + public char getDSNumber() { + return '1'; + } + + @Override + public boolean isFMSConnected() { + return false; + } + + @Override + public boolean isEnabled() { + return false; + } + + @Override + public ControlMode getControlMode() { + return ControlMode.TELEOP; + } + + @Override + public void updateBattery(char[] batteryData) { + return; + } + + @Override + public void updateRobotFeedback(boolean[] fromRobot) { + return; + } + + @Override + public void setRobotConnected(boolean connected) { + return; + } +} diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/robotcoms/ControlMode.java b/driver-station/src/main/java/edu/wpi/first/driverstation/robotcoms/ControlMode.java new file mode 100644 index 0000000000..b143c0b87d --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/robotcoms/ControlMode.java @@ -0,0 +1,24 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package edu.wpi.first.driverstation.robotcoms; + +/** + * + * @author Brendan + */ +public enum ControlMode { + AUTO("AUTO"), TELEOP("TELEOP"), TEST_MODE("TEST"); + private String name; + + private ControlMode(String name) { + this.name = name; + } + + @Override + public String toString() { + return name; //To change body of generated methods, choose Tools | Templates. + } + +} diff --git a/driver-station/src/main/java/edu/wpi/first/driverstation/robotcoms/RobotComm.java b/driver-station/src/main/java/edu/wpi/first/driverstation/robotcoms/RobotComm.java new file mode 100644 index 0000000000..116ee12e70 --- /dev/null +++ b/driver-station/src/main/java/edu/wpi/first/driverstation/robotcoms/RobotComm.java @@ -0,0 +1,319 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package edu.wpi.first.driverstation.robotcoms; + +import edu.wpi.first.driverstation.DriverStation; +import edu.wpi.first.driverstation.fms.FMSInterface; +import edu.wpi.first.driverstation.fms.NoFMSController; + +import java.net.*; +import java.util.logging.Level; +import java.util.logging.Logger; +import java.util.zip.CRC32; + +/** + * + * @author Brendan + */ +public class RobotComm extends Thread { + + private static final Logger log = Logger.getLogger(DriverStation.class + .getName()); + + private class RobotDataIn extends Thread { + + RobotComm theRobotComm; + private DatagramSocket robotReceiveSocket; + private int robotReceivePort = 1150; + + public RobotDataIn(RobotComm robotComm) { + theRobotComm = robotComm; + try { + robotReceiveSocket = new DatagramSocket(robotReceivePort); + } catch (SocketException ex) { + log.log(Level.WARNING, "Error creating Robot Sockets", ex); + } + } + + public void run() { + while (true) { + try { + byte[] receiveData = new byte[1024];// the size of the data + DatagramPacket receivePacket = new DatagramPacket( + receiveData, receiveData.length); + robotReceiveSocket.receive(receivePacket); + theRobotComm.receiveRobotpacket(receiveData); + Thread.yield(); + } catch (Exception ex) { + } + } + } + } + + private DatagramSocket robotSendingSocket; + private RobotDataIn inputSocket; + private final int robotSendPortNormal = 1110; + // private final int robotSendPortNormal = 1115;//actually for sending on + // the field (used for robot sim) + private final int robotSendPortFMS = 1115; + private int sentPackets;// the current packet ID that is being sent to the + // robot + private int lastRobotPacket;// the last packet ID to have been recieved by + // the robot + private long lastRobotTime;// the time stamp when the robot was seen + private boolean[] controlDataToSend;// {auto, test mode, enabled, + // not-estopped, reboot} + private boolean[] controlFromRobot;// {auto, test mode, enabled, + // not-estopped} + private byte[] stationID = new byte[2]; + private char[] batteryData = new char[2]; + private int robotTeam = 0; + private long lastSentTime = 0; + private int packetOffset = 0; + private FMSInterface fmsController; + + public RobotComm(int team) { + + robotTeam = team; + try { + robotSendingSocket = new DatagramSocket();// robotSendPortNormal); + } catch (SocketException ex) { + log.log(Level.WARNING, "Error creating Robot Sockets", ex); + } + sentPackets = 0; + controlFromRobot = new boolean[] { false, false, false, true };// {auto, + // test + // mode, + // enabled, + // not-estopped} + + controlDataToSend = new boolean[] { false, false, false, true, false };// {auto, + // test + // mode, + // enabled, + // not-estopped, + // reboot} + fmsController = new NoFMSController(); + inputSocket = new RobotDataIn(this); + inputSocket.setDaemon(true); + } + + @Override + public void start() { + inputSocket.start(); + super.start(); + } + + @Override + public void run() { + while (true) { + if ((lastSentTime + 20) < System.currentTimeMillis()) { + lastSentTime = System.currentTimeMillis(); + sendRobotData(); + } + + } + } + + public void rebootRobot() { + setEnabled(false); + controlDataToSend[4] = true; + } + + public void setControlMode(ControlMode theMode) { + if (theMode == ControlMode.AUTO) {// {auto, test mode, enabled, + // not-estopped} + controlDataToSend[0] = true; + controlDataToSend[1] = false; + } else if (theMode == ControlMode.TELEOP) {// {auto, test mode, enabled, + // not-estopped} + controlDataToSend[0] = false; + controlDataToSend[1] = false; + } else if (theMode == ControlMode.TEST_MODE) {// {auto, test mode, + // enabled, + // not-estopped} + controlDataToSend[0] = false; + controlDataToSend[1] = true; + } + } + + public void setEnabled(boolean enabled) {// {auto, test mode, enabled, + // not-estopped, reboot} + controlDataToSend[2] = enabled; + } + + public void eStopRobot() { + + controlDataToSend[3] = false; + } + + public void setAllianceInfo(byte allianceColor, byte allianceStation) { + stationID = new byte[] { allianceColor, allianceStation }; + } + + public void setTeamNumber(int newTeam) { + robotTeam = newTeam; + } + + public void setFMSController(FMSInterface newFMS) { + fmsController = newFMS; + } + + /** + * Packs up all the data and sends the packet to the robot + */ + private void sendRobotData() { + sentPackets++; + sentPackets = sentPackets % 0xFFFF;// makes it overflow properly + packetOffset = sentPackets - lastRobotPacket; + CRC32 crctest = new CRC32(); + byte[] sendData = new byte[1024]; + + for (int i = 0; i < 1024; i++) { + sendData[i] = 0x00; + } + + // Construct the pacet counts + sendData[0] = (byte) ((sentPackets >> 8) & 0xFF); + sendData[1] = (byte) (sentPackets & 0xFF); + // {auto, test mode, enabled, not-estopped, reboot} + if (controlDataToSend[4]) {// reboot bit + sendData[2] += 0x80; + + controlDataToSend[4] = false; + } + if (controlDataToSend[3]) {// not e-stop bit + sendData[2] += 0x40; + } else { + controlDataToSend[3] = true; + } + if (controlDataToSend[2])// if enabled + { + sendData[2] += 0x20; + } + if (controlDataToSend[0])// 1 holds the mode (1=auto 0=teleop) + { + sendData[2] += 0x10; + } + if (fmsController.isFMSConnected()) { + sendData[2] += 0x08; + } + + if (!isConnected()) { + sendData[2] += 0x04; + } + + sendData[3] = (byte) 0xFF;// make all digial inputs high + + sendData[4] = (byte) ((robotTeam >> 8) & 0xFF);// packs up and sends + // team number + sendData[5] = (byte) ((robotTeam) & 0xFF); + + sendData[6] = stationID[0];// the alliance color + sendData[7] = stationID[1]; // the station number + + sendData[72] = (byte) 0x31; + sendData[73] = (byte) 0x31; + sendData[74] = (byte) 0x33; + sendData[75] = (byte) 0x30; + sendData[76] = (byte) 0x21; + sendData[77] = (byte) 0x21; + sendData[78] = (byte) 0x30; + sendData[79] = (byte) 0x30; + + crctest.reset(); + crctest.update(sendData); + long crccheck = crctest.getValue(); + sendData[1020] = (byte) ((crccheck >> 24) & 0xff); + sendData[1021] = (byte) ((crccheck >> 16) & 0xff); + sendData[1022] = (byte) ((crccheck >> 8) & 0xff); + sendData[1023] = (byte) ((crccheck >> 0) & 0xff); + + byte teamHigh = (byte) ((robotTeam - robotTeam % 100) / 100); + byte teamLow = (byte) (robotTeam % 100); + /* + * send packet + */ + try { + DatagramPacket sendPacket; + if (!fmsController.isFMSConnected()) { + sendPacket = new DatagramPacket(sendData, sendData.length, + InetAddress.getByAddress(new byte[] { 10, teamHigh, + teamLow, 2 }), robotSendPortNormal); + } else { + sendPacket = new DatagramPacket(sendData, sendData.length, + InetAddress.getByAddress(new byte[] { 10, teamHigh, + teamLow, 2 }), robotSendPortFMS); + } + robotSendingSocket.send(sendPacket); + } catch (Exception e) { + log.log(Level.WARNING, "Failed to send", e); + } + + fmsController.setRobotConnected(isConnected()); + + } + + public boolean isConnected() { + + if (!(lastRobotTime + 100 > System.currentTimeMillis())) { + return (false); + } + + if (packetOffset > 25) { + return (false); + } + + return (true); + } + + public char[] getBatteryData() { + return batteryData; + } + + /** + * Return the control data that it has gathered from the robot in the + * format: {auto, test mode, enabled, not-estopped} + * + * @return The data from the robot + */ + public boolean[] getControlDataFromRobot() { + return controlFromRobot; + } + + public double getBatteryVoltage() { + return Double.parseDouble(getBatteryString()); + } + + public String getBatteryString() { + return (String.format("%x", getBatteryData()[0]) + "." + String.format( + "%x", getBatteryData()[1])); + } + + protected void receiveRobotpacket(byte[] dataIn) {// this gets called + // whenever a new robot + // packet is recieved + int teamNumber = 0; + teamNumber += dataIn[8] & 0xFF; + teamNumber = ((teamNumber << 8) + (dataIn[9] & 0xFF)) & 0xFFFF; + if (robotTeam == teamNumber) { + lastRobotTime = System.currentTimeMillis(); + batteryData[0] = (char) dataIn[1]; + batteryData[1] = (char) dataIn[2]; + lastRobotPacket = 0; + lastRobotPacket += dataIn[30] & 0xFF; + lastRobotPacket = ((lastRobotPacket << 8) + (dataIn[31] & 0xFF)) & 0xFFFF; + controlFromRobot[0] = ((0x10 & dataIn[0]) > 0);// auto + controlFromRobot[2] = ((0x20 & dataIn[0]) > 0);// enabled + controlFromRobot[3] = true;// robot isn't estopped yet + + } else { + log.log(Level.INFO, "Data from different team..."); + } + fmsController.updateRobotFeedback(controlFromRobot); + fmsController.updateBattery(batteryData); + + } +} diff --git a/pom.xml b/pom.xml index 666dff8fa8..bff1e22832 100644 --- a/pom.xml +++ b/pom.xml @@ -11,6 +11,7 @@ networktables wpilibj eclipse-plugins + driver-station