mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Driver Station
Added a java implementation of the FRC Network Communication Protocol. The majority of network related code was written by Brendan McLeod, and I have adapted a front end that listens for input from standard input. Change-Id: Id9e4d5b52425e56bf8a848b17390fb83b9e75d29
This commit is contained in:
Fredric Silberberg
1
.gitignore
vendored
1
.gitignore
vendored
@@ -3,6 +3,7 @@ hal/build/
|
||||
networktables/cpp/build/
|
||||
build/
|
||||
networktables/OutlineViewer/nbproject/private
|
||||
*~
|
||||
target/
|
||||
dist/
|
||||
bin/
|
||||
|
||||
44
driver-station/pom.xml
Normal file
44
driver-station/pom.xml
Normal file
@@ -0,0 +1,44 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd" xmlns="http://maven.apache.org/POM/4.0.0"
|
||||
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
|
||||
<modelVersion>4.0.0</modelVersion>
|
||||
<groupId>edu.wpi.first</groupId>
|
||||
<artifactId>driver-station</artifactId>
|
||||
<packaging>jar</packaging>
|
||||
<version>0.1.0-SNAPSHOT</version>
|
||||
<profiles>
|
||||
<profile>
|
||||
<id>docline-java8-disable</id>
|
||||
<activation>
|
||||
<jdk>[1.8,</jdk>
|
||||
</activation>
|
||||
</profile>
|
||||
</profiles>
|
||||
|
||||
<build>
|
||||
<plugins>
|
||||
<plugin>
|
||||
<groupId>org.apache.maven.plugins</groupId>
|
||||
<artifactId>maven-compiler-plugin</artifactId>
|
||||
<version>3.1</version>
|
||||
<configuration>
|
||||
<source>1.7</source>
|
||||
<target>1.7</target>
|
||||
</configuration>
|
||||
</plugin>
|
||||
<plugin>
|
||||
<groupId>org.apache.maven.plugins</groupId>
|
||||
<artifactId>maven-jar-plugin</artifactId>
|
||||
<version>2.4</version>
|
||||
<configuration>
|
||||
<archive>
|
||||
<manifest>
|
||||
<mainClass>edu.wpi.first.driverstation.DriverStation</mainClass>
|
||||
</manifest>
|
||||
</archive>
|
||||
</configuration>
|
||||
</plugin>
|
||||
</plugins>
|
||||
</build>
|
||||
|
||||
</project>
|
||||
@@ -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<String> 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 <driverstation 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<String> 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<Thread> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user