Squashed 'third_party/allwpilib_2019/' content from commit bd05dfa1c
Change-Id: I2b1c2250cdb9b055133780c33593292098c375b7
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: bd05dfa1c7cca74c4fac451e7b9d6a37e7b53447
diff --git a/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java b/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
new file mode 100644
index 0000000..2c08b17
--- /dev/null
+++ b/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpiutil.RuntimeDetector;
+
+public final class DevMain {
+ /**
+ * Main entry point.
+ */
+ public static void main(String[] args) {
+ System.out.println("Hello World!");
+ System.out.println(RuntimeDetector.getPlatformPath());
+ System.out.println(NetworkTablesJNI.now());
+ System.out.println(HALUtil.getHALRuntimeType());
+ }
+
+ private DevMain() {
+ }
+}
diff --git a/wpilibj/src/dev/native/cpp/main.cpp b/wpilibj/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..e324b44
--- /dev/null
+++ b/wpilibj/src/dev/native/cpp/main.cpp
@@ -0,0 +1,8 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+int main() {}
diff --git a/wpilibj/src/generate/WPILibVersion.java.in b/wpilibj/src/generate/WPILibVersion.java.in
new file mode 100644
index 0000000..cc5e6b0
--- /dev/null
+++ b/wpilibj/src/generate/WPILibVersion.java.in
@@ -0,0 +1,10 @@
+package edu.wpi.first.wpilibj.util;
+
+/*
+ * Autogenerated file! Do not manually edit this file. This version is regenerated
+ * any time the publish task is run, or when this file is deleted.
+ */
+
+public final class WPILibVersion {
+ public static final String Version = "${wpilib_version}";
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
new file mode 100644
index 0000000..e91b8d2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * ADXL345 I2C Accelerometer.
+ */
+@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
+public class ADXL345_I2C extends SendableBase implements Accelerometer {
+ private static final byte kAddress = 0x1D;
+ private static final byte kPowerCtlRegister = 0x2D;
+ private static final byte kDataFormatRegister = 0x31;
+ private static final byte kDataRegister = 0x32;
+ private static final double kGsPerLSB = 0.00390625;
+ private static final byte kPowerCtl_Link = 0x20;
+ private static final byte kPowerCtl_AutoSleep = 0x10;
+ private static final byte kPowerCtl_Measure = 0x08;
+ private static final byte kPowerCtl_Sleep = 0x04;
+
+ private static final byte kDataFormat_SelfTest = (byte) 0x80;
+ private static final byte kDataFormat_SPI = 0x40;
+ private static final byte kDataFormat_IntInvert = 0x20;
+ private static final byte kDataFormat_FullRes = 0x08;
+ private static final byte kDataFormat_Justify = 0x04;
+
+ public enum Axes {
+ kX((byte) 0x00),
+ kY((byte) 0x02),
+ kZ((byte) 0x04);
+
+ /**
+ * The integer value representing this enumeration.
+ */
+ @SuppressWarnings("MemberName")
+ public final byte value;
+
+ Axes(byte value) {
+ this.value = value;
+ }
+ }
+
+ @SuppressWarnings("MemberName")
+ public static class AllAxes {
+ public double XAxis;
+ public double YAxis;
+ public double ZAxis;
+ }
+
+ protected I2C m_i2c;
+
+ /**
+ * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
+ *
+ * @param port The I2C port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL345_I2C(I2C.Port port, Range range) {
+ this(port, range, kAddress);
+ }
+
+ /**
+ * Constructs the ADXL345 Accelerometer over I2C.
+ *
+ * @param port The I2C port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
+ */
+ public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
+ m_i2c = new I2C(port, deviceAddress);
+
+ // Turn on the measurements
+ m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
+
+ setRange(range);
+
+ HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
+ setName("ADXL345_I2C", port.value);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ m_i2c.close();
+ }
+
+ @Override
+ public void setRange(Range range) {
+ final byte value;
+
+ switch (range) {
+ case k2G:
+ value = 0;
+ break;
+ case k4G:
+ value = 1;
+ break;
+ case k8G:
+ value = 2;
+ break;
+ case k16G:
+ value = 3;
+ break;
+ default:
+ throw new IllegalArgumentException(range + " unsupported range type");
+ }
+
+ // Specify the data format to read
+ m_i2c.write(kDataFormatRegister, kDataFormat_FullRes | value);
+ }
+
+ @Override
+ public double getX() {
+ return getAcceleration(Axes.kX);
+ }
+
+ @Override
+ public double getY() {
+ return getAcceleration(Axes.kY);
+ }
+
+ @Override
+ public double getZ() {
+ return getAcceleration(Axes.kZ);
+ }
+
+ /**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL345 in Gs.
+ */
+ public double getAcceleration(Axes axis) {
+ ByteBuffer rawAccel = ByteBuffer.allocate(2);
+ m_i2c.read(kDataRegister + axis.value, 2, rawAccel);
+
+ // Sensor is little endian... swap bytes
+ rawAccel.order(ByteOrder.LITTLE_ENDIAN);
+ return rawAccel.getShort(0) * kGsPerLSB;
+ }
+
+ /**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
+ */
+ public AllAxes getAccelerations() {
+ AllAxes data = new AllAxes();
+ ByteBuffer rawData = ByteBuffer.allocate(6);
+ m_i2c.read(kDataRegister, 6, rawData);
+
+ // Sensor is little endian... swap bytes
+ rawData.order(ByteOrder.LITTLE_ENDIAN);
+ data.XAxis = rawData.getShort(0) * kGsPerLSB;
+ data.YAxis = rawData.getShort(2) * kGsPerLSB;
+ data.ZAxis = rawData.getShort(4) * kGsPerLSB;
+ return data;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ NetworkTableEntry entryX = builder.getEntry("X");
+ NetworkTableEntry entryY = builder.getEntry("Y");
+ NetworkTableEntry entryZ = builder.getEntry("Z");
+ builder.setUpdateTable(() -> {
+ AllAxes data = getAccelerations();
+ entryX.setDouble(data.XAxis);
+ entryY.setDouble(data.YAxis);
+ entryZ.setDouble(data.ZAxis);
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
new file mode 100644
index 0000000..b5d3618
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -0,0 +1,203 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * ADXL345 SPI Accelerometer.
+ */
+@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
+public class ADXL345_SPI extends SendableBase implements Accelerometer {
+ private static final int kPowerCtlRegister = 0x2D;
+ private static final int kDataFormatRegister = 0x31;
+ private static final int kDataRegister = 0x32;
+ private static final double kGsPerLSB = 0.00390625;
+
+ private static final int kAddress_Read = 0x80;
+ private static final int kAddress_MultiByte = 0x40;
+
+ private static final int kPowerCtl_Link = 0x20;
+ private static final int kPowerCtl_AutoSleep = 0x10;
+ private static final int kPowerCtl_Measure = 0x08;
+ private static final int kPowerCtl_Sleep = 0x04;
+
+ private static final int kDataFormat_SelfTest = 0x80;
+ private static final int kDataFormat_SPI = 0x40;
+ private static final int kDataFormat_IntInvert = 0x20;
+ private static final int kDataFormat_FullRes = 0x08;
+ private static final int kDataFormat_Justify = 0x04;
+
+ public enum Axes {
+ kX((byte) 0x00),
+ kY((byte) 0x02),
+ kZ((byte) 0x04);
+
+ /**
+ * The integer value representing this enumeration.
+ */
+ @SuppressWarnings("MemberName")
+ public final byte value;
+
+ Axes(byte value) {
+ this.value = value;
+ }
+ }
+
+ @SuppressWarnings("MemberName")
+ public static class AllAxes {
+ public double XAxis;
+ public double YAxis;
+ public double ZAxis;
+ }
+
+ protected SPI m_spi;
+
+ /**
+ * Constructor.
+ *
+ * @param port The SPI port that the accelerometer is connected to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL345_SPI(SPI.Port port, Range range) {
+ m_spi = new SPI(port);
+ init(range);
+ setName("ADXL345_SPI", port.value);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ m_spi.close();
+ }
+
+ /**
+ * Set SPI bus parameters, bring device out of sleep and set format.
+ *
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ private void init(Range range) {
+ m_spi.setClockRate(500000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnTrailingEdge();
+ m_spi.setClockActiveLow();
+ m_spi.setChipSelectActiveHigh();
+
+ // Turn on the measurements
+ byte[] commands = new byte[2];
+ commands[0] = kPowerCtlRegister;
+ commands[1] = kPowerCtl_Measure;
+ m_spi.write(commands, 2);
+
+ setRange(range);
+
+ HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_SPI);
+ }
+
+ @Override
+ public void setRange(Range range) {
+ final byte value;
+
+ switch (range) {
+ case k2G:
+ value = 0;
+ break;
+ case k4G:
+ value = 1;
+ break;
+ case k8G:
+ value = 2;
+ break;
+ case k16G:
+ value = 3;
+ break;
+ default:
+ throw new IllegalArgumentException(range + " unsupported");
+ }
+
+ // Specify the data format to read
+ byte[] commands = new byte[]{kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
+ m_spi.write(commands, commands.length);
+ }
+
+ @Override
+ public double getX() {
+ return getAcceleration(Axes.kX);
+ }
+
+ @Override
+ public double getY() {
+ return getAcceleration(Axes.kY);
+ }
+
+ @Override
+ public double getZ() {
+ return getAcceleration(Axes.kZ);
+ }
+
+ /**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL345 in Gs.
+ */
+ public double getAcceleration(ADXL345_SPI.Axes axis) {
+ ByteBuffer transferBuffer = ByteBuffer.allocate(3);
+ transferBuffer.put(0,
+ (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
+ m_spi.transaction(transferBuffer, transferBuffer, 3);
+ // Sensor is little endian
+ transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ return transferBuffer.getShort(1) * kGsPerLSB;
+ }
+
+ /**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
+ */
+ public ADXL345_SPI.AllAxes getAccelerations() {
+ ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
+ if (m_spi != null) {
+ ByteBuffer dataBuffer = ByteBuffer.allocate(7);
+ // Select the data address.
+ dataBuffer.put(0, (byte) (kAddress_Read | kAddress_MultiByte | kDataRegister));
+ m_spi.transaction(dataBuffer, dataBuffer, 7);
+ // Sensor is little endian... swap bytes
+ dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ data.XAxis = dataBuffer.getShort(1) * kGsPerLSB;
+ data.YAxis = dataBuffer.getShort(3) * kGsPerLSB;
+ data.ZAxis = dataBuffer.getShort(5) * kGsPerLSB;
+ }
+ return data;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ NetworkTableEntry entryX = builder.getEntry("X");
+ NetworkTableEntry entryY = builder.getEntry("Y");
+ NetworkTableEntry entryZ = builder.getEntry("Z");
+ builder.setUpdateTable(() -> {
+ AllAxes data = getAccelerations();
+ entryX.setDouble(data.XAxis);
+ entryY.setDouble(data.YAxis);
+ entryZ.setDouble(data.ZAxis);
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
new file mode 100644
index 0000000..5324b63
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -0,0 +1,227 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * ADXL362 SPI Accelerometer.
+ *
+ * <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
+ */
+@SuppressWarnings("PMD.UnusedPrivateField")
+public class ADXL362 extends SendableBase implements Accelerometer {
+ private static final byte kRegWrite = 0x0A;
+ private static final byte kRegRead = 0x0B;
+
+ private static final byte kPartIdRegister = 0x02;
+ private static final byte kDataRegister = 0x0E;
+ private static final byte kFilterCtlRegister = 0x2C;
+ private static final byte kPowerCtlRegister = 0x2D;
+
+ private static final byte kFilterCtl_Range2G = 0x00;
+ private static final byte kFilterCtl_Range4G = 0x40;
+ private static final byte kFilterCtl_Range8G = (byte) 0x80;
+ private static final byte kFilterCtl_ODR_100Hz = 0x03;
+
+ private static final byte kPowerCtl_UltraLowNoise = 0x20;
+ private static final byte kPowerCtl_AutoSleep = 0x04;
+ private static final byte kPowerCtl_Measure = 0x02;
+
+ public enum Axes {
+ kX((byte) 0x00),
+ kY((byte) 0x02),
+ kZ((byte) 0x04);
+
+ @SuppressWarnings("MemberName")
+ public final byte value;
+
+ Axes(byte value) {
+ this.value = value;
+ }
+ }
+
+ @SuppressWarnings("MemberName")
+ public static class AllAxes {
+ public double XAxis;
+ public double YAxis;
+ public double ZAxis;
+ }
+
+ private SPI m_spi;
+ private double m_gsPerLSB;
+
+ /**
+ * Constructor. Uses the onboard CS1.
+ *
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL362(Range range) {
+ this(SPI.Port.kOnboardCS1, range);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param port The SPI port that the accelerometer is connected to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ public ADXL362(SPI.Port port, Range range) {
+ m_spi = new SPI(port);
+
+ m_spi.setClockRate(3000000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnTrailingEdge();
+ m_spi.setClockActiveLow();
+ m_spi.setChipSelectActiveLow();
+
+ // Validate the part ID
+ ByteBuffer transferBuffer = ByteBuffer.allocate(3);
+ transferBuffer.put(0, kRegRead);
+ transferBuffer.put(1, kPartIdRegister);
+ m_spi.transaction(transferBuffer, transferBuffer, 3);
+ if (transferBuffer.get(2) != (byte) 0xF2) {
+ m_spi.close();
+ m_spi = null;
+ DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
+ return;
+ }
+
+ setRange(range);
+
+ // Turn on the measurements
+ transferBuffer.put(0, kRegWrite);
+ transferBuffer.put(1, kPowerCtlRegister);
+ transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise));
+ m_spi.write(transferBuffer, 3);
+
+ HAL.report(tResourceType.kResourceType_ADXL362, port.value);
+ setName("ADXL362", port.value);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ if (m_spi != null) {
+ m_spi.close();
+ m_spi = null;
+ }
+ }
+
+ @Override
+ public void setRange(Range range) {
+ if (m_spi == null) {
+ return;
+ }
+
+ final byte value;
+ switch (range) {
+ case k2G:
+ value = kFilterCtl_Range2G;
+ m_gsPerLSB = 0.001;
+ break;
+ case k4G:
+ value = kFilterCtl_Range4G;
+ m_gsPerLSB = 0.002;
+ break;
+ case k8G:
+ case k16G: // 16G not supported; treat as 8G
+ value = kFilterCtl_Range8G;
+ m_gsPerLSB = 0.004;
+ break;
+ default:
+ throw new IllegalArgumentException(range + " unsupported");
+
+ }
+
+ // Specify the data format to read
+ byte[] commands = new byte[]{kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz
+ | value)};
+ m_spi.write(commands, commands.length);
+ }
+
+
+ @Override
+ public double getX() {
+ return getAcceleration(Axes.kX);
+ }
+
+ @Override
+ public double getY() {
+ return getAcceleration(Axes.kY);
+ }
+
+ @Override
+ public double getZ() {
+ return getAcceleration(Axes.kZ);
+ }
+
+ /**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL362 in Gs.
+ */
+ public double getAcceleration(ADXL362.Axes axis) {
+ if (m_spi == null) {
+ return 0.0;
+ }
+ ByteBuffer transferBuffer = ByteBuffer.allocate(4);
+ transferBuffer.put(0, kRegRead);
+ transferBuffer.put(1, (byte) (kDataRegister + axis.value));
+ m_spi.transaction(transferBuffer, transferBuffer, 4);
+ // Sensor is little endian
+ transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ return transferBuffer.getShort(2) * m_gsPerLSB;
+ }
+
+ /**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the ADXL362 in Gs.
+ */
+ public ADXL362.AllAxes getAccelerations() {
+ ADXL362.AllAxes data = new ADXL362.AllAxes();
+ if (m_spi != null) {
+ ByteBuffer dataBuffer = ByteBuffer.allocate(8);
+ // Select the data address.
+ dataBuffer.put(0, kRegRead);
+ dataBuffer.put(1, kDataRegister);
+ m_spi.transaction(dataBuffer, dataBuffer, 8);
+ // Sensor is little endian... swap bytes
+ dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
+
+ data.XAxis = dataBuffer.getShort(2) * m_gsPerLSB;
+ data.YAxis = dataBuffer.getShort(4) * m_gsPerLSB;
+ data.ZAxis = dataBuffer.getShort(6) * m_gsPerLSB;
+ }
+ return data;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ NetworkTableEntry entryX = builder.getEntry("X");
+ NetworkTableEntry entryY = builder.getEntry("Y");
+ NetworkTableEntry entryZ = builder.getEntry("Z");
+ builder.setUpdateTable(() -> {
+ AllAxes data = getAccelerations();
+ entryX.setDouble(data.XAxis);
+ entryY.setDouble(data.YAxis);
+ entryZ.setDouble(data.ZAxis);
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
new file mode 100644
index 0000000..a34b373
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -0,0 +1,167 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
+ * tracks the robots heading based on the starting position. As the robot rotates the new heading is
+ * computed by integrating the rate of rotation returned by the sensor. When the class is
+ * instantiated, it does a short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to determine the heading.
+ *
+ * <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
+public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable {
+ private static final double kSamplePeriod = 0.0005;
+ private static final double kCalibrationSampleTime = 5.0;
+ private static final double kDegreePerSecondPerLSB = 0.0125;
+
+ private static final int kRateRegister = 0x00;
+ private static final int kTemRegister = 0x02;
+ private static final int kLoCSTRegister = 0x04;
+ private static final int kHiCSTRegister = 0x06;
+ private static final int kQuadRegister = 0x08;
+ private static final int kFaultRegister = 0x0A;
+ private static final int kPIDRegister = 0x0C;
+ private static final int kSNHighRegister = 0x0E;
+ private static final int kSNLowRegister = 0x10;
+
+ private SPI m_spi;
+
+ /**
+ * Constructor. Uses the onboard CS0.
+ */
+ public ADXRS450_Gyro() {
+ this(SPI.Port.kOnboardCS0);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param port The SPI port that the gyro is connected to
+ */
+ public ADXRS450_Gyro(SPI.Port port) {
+ m_spi = new SPI(port);
+
+ m_spi.setClockRate(3000000);
+ m_spi.setMSBFirst();
+ m_spi.setSampleDataOnLeadingEdge();
+ m_spi.setClockActiveHigh();
+ m_spi.setChipSelectActiveLow();
+
+ // Validate the part ID
+ if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
+ m_spi.close();
+ m_spi = null;
+ DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
+ false);
+ return;
+ }
+
+ m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
+ true, true);
+
+ calibrate();
+
+ HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
+ setName("ADXRS450_Gyro", port.value);
+ }
+
+ public boolean isConnected() {
+ return m_spi != null;
+ }
+
+ @Override
+ public void calibrate() {
+ if (m_spi == null) {
+ return;
+ }
+
+ Timer.delay(0.1);
+
+ m_spi.setAccumulatorIntegratedCenter(0);
+ m_spi.resetAccumulator();
+
+ Timer.delay(kCalibrationSampleTime);
+
+ m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage());
+ m_spi.resetAccumulator();
+ }
+
+ private boolean calcParity(int value) {
+ boolean parity = false;
+ while (value != 0) {
+ parity = !parity;
+ value = value & (value - 1);
+ }
+ return parity;
+ }
+
+ private int readRegister(int reg) {
+ int cmdhi = 0x8000 | (reg << 1);
+ boolean parity = calcParity(cmdhi);
+
+ ByteBuffer buf = ByteBuffer.allocate(4);
+ buf.order(ByteOrder.BIG_ENDIAN);
+ buf.put(0, (byte) (cmdhi >> 8));
+ buf.put(1, (byte) (cmdhi & 0xff));
+ buf.put(2, (byte) 0);
+ buf.put(3, (byte) (parity ? 0 : 1));
+
+ m_spi.write(buf, 4);
+ m_spi.read(false, buf, 4);
+
+ if ((buf.get(0) & 0xe0) == 0) {
+ return 0; // error, return 0
+ }
+ return (buf.getInt(0) >> 5) & 0xffff;
+ }
+
+ @Override
+ public void reset() {
+ if (m_spi != null) {
+ m_spi.resetAccumulator();
+ }
+ }
+
+ /**
+ * Delete (free) the spi port used for the gyro and stop accumulating.
+ */
+ @Override
+ public void close() {
+ super.close();
+ if (m_spi != null) {
+ m_spi.close();
+ m_spi = null;
+ }
+ }
+
+ @Override
+ public double getAngle() {
+ if (m_spi == null) {
+ return 0.0;
+ }
+ return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
+ }
+
+ @Override
+ public double getRate() {
+ if (m_spi == null) {
+ return 0.0;
+ }
+ return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
new file mode 100644
index 0000000..cd64dc8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -0,0 +1,143 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Handle operation of an analog accelerometer. The accelerometer reads acceleration directly
+ * through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each
+ * is calibrated by finding the center value over a period of time.
+ */
+public class AnalogAccelerometer extends SendableBase implements PIDSource {
+ private AnalogInput m_analogChannel;
+ private double m_voltsPerG = 1.0;
+ private double m_zeroGVoltage = 2.5;
+ private final boolean m_allocatedChannel;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Common initialization.
+ */
+ private void initAccelerometer() {
+ HAL.report(tResourceType.kResourceType_Accelerometer,
+ m_analogChannel.getChannel());
+ setName("Accelerometer", m_analogChannel.getChannel());
+ }
+
+ /**
+ * Create a new instance of an accelerometer.
+ *
+ * <p>The constructor allocates desired analog channel.
+ *
+ * @param channel The channel number for the analog input the accelerometer is connected to
+ */
+ public AnalogAccelerometer(final int channel) {
+ this(new AnalogInput(channel), true);
+ addChild(m_analogChannel);
+ }
+
+ /**
+ * Create a new instance of Accelerometer from an existing AnalogChannel. Make a new instance of
+ * accelerometer given an AnalogChannel. This is particularly useful if the port is going to be
+ * read as an analog channel as well as through the Accelerometer class.
+ *
+ * @param channel The existing AnalogInput object for the analog input the accelerometer is
+ * connected to
+ */
+ public AnalogAccelerometer(final AnalogInput channel) {
+ this(channel, false);
+ }
+
+ private AnalogAccelerometer(final AnalogInput channel, final boolean allocatedChannel) {
+ requireNonNull(channel, "Analog Channel given was null");
+ m_allocatedChannel = allocatedChannel;
+ m_analogChannel = channel;
+ initAccelerometer();
+ }
+
+ /**
+ * Delete the analog components used for the accelerometer.
+ */
+ @Override
+ public void close() {
+ super.close();
+ if (m_analogChannel != null && m_allocatedChannel) {
+ m_analogChannel.close();
+ }
+ m_analogChannel = null;
+ }
+
+ /**
+ * Return the acceleration in Gs.
+ *
+ * <p>The acceleration is returned units of Gs.
+ *
+ * @return The current acceleration of the sensor in Gs.
+ */
+ public double getAcceleration() {
+ if (m_analogChannel == null) {
+ return 0.0;
+ }
+ return (m_analogChannel.getAverageVoltage() - m_zeroGVoltage) / m_voltsPerG;
+ }
+
+ /**
+ * Set the accelerometer sensitivity.
+ *
+ * <p>This sets the sensitivity of the accelerometer used for calculating the acceleration. The
+ * sensitivity varies by accelerometer model. There are constants defined for various models.
+ *
+ * @param sensitivity The sensitivity of accelerometer in Volts per G.
+ */
+ public void setSensitivity(double sensitivity) {
+ m_voltsPerG = sensitivity;
+ }
+
+ /**
+ * Set the voltage that corresponds to 0 G.
+ *
+ * <p>The zero G voltage varies by accelerometer model. There are constants defined for various
+ * models.
+ *
+ * @param zero The zero G voltage.
+ */
+ public void setZero(double zero) {
+ m_zeroGVoltage = zero;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the Acceleration for the PID Source parent.
+ *
+ * @return The current acceleration in Gs.
+ */
+ @Override
+ public double pidGet() {
+ return getAcceleration();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Accelerometer");
+ builder.addDoubleProperty("Value", this::getAcceleration, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
new file mode 100644
index 0000000..4283aa0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -0,0 +1,189 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogGyroJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
+ * tracks the robots heading based on the starting position. As the robot rotates the new heading is
+ * computed by integrating the rate of rotation returned by the sensor. When the class is
+ * instantiated, it does a short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to determine the heading.
+ *
+ * <p>This class is for gyro sensors that connect to an analog input.
+ */
+public class AnalogGyro extends GyroBase implements Gyro, PIDSource, Sendable {
+ private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
+ protected AnalogInput m_analog;
+ private boolean m_channelAllocated;
+
+ private int m_gyroHandle;
+
+ /**
+ * Initialize the gyro. Calibration is handled by calibrate().
+ */
+ public void initGyro() {
+ if (m_gyroHandle == 0) {
+ m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
+ }
+
+ AnalogGyroJNI.setupAnalogGyro(m_gyroHandle);
+
+ HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
+ setName("AnalogGyro", m_analog.getChannel());
+ }
+
+ @Override
+ public void calibrate() {
+ AnalogGyroJNI.calibrateAnalogGyro(m_gyroHandle);
+ }
+
+ /**
+ * Gyro constructor using the channel number.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
+ * channels 0-1.
+ */
+ public AnalogGyro(int channel) {
+ this(new AnalogInput(channel));
+ m_channelAllocated = true;
+ addChild(m_analog);
+ }
+
+ /**
+ * Gyro constructor with a precreated analog channel object. Use this constructor when the analog
+ * channel needs to be shared.
+ *
+ * @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
+ * on-board channels 0-1.
+ */
+ public AnalogGyro(AnalogInput channel) {
+ requireNonNull(channel, "AnalogInput supplied to Gyro constructor is null");
+
+ m_analog = channel;
+ initGyro();
+ calibrate();
+ }
+
+ /**
+ * Gyro constructor using the channel number along with parameters for presetting the center and
+ * offset values. Bypasses calibration.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
+ * channels 0-1.
+ * @param center Preset uncalibrated value to use as the accumulator center value.
+ * @param offset Preset uncalibrated value to use as the gyro offset.
+ */
+ public AnalogGyro(int channel, int center, double offset) {
+ this(new AnalogInput(channel), center, offset);
+ m_channelAllocated = true;
+ addChild(m_analog);
+ }
+
+ /**
+ * Gyro constructor with a precreated analog channel object along with parameters for presetting
+ * the center and offset values. Bypasses calibration.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
+ * channels 0-1.
+ * @param center Preset uncalibrated value to use as the accumulator center value.
+ * @param offset Preset uncalibrated value to use as the gyro offset.
+ */
+ public AnalogGyro(AnalogInput channel, int center, double offset) {
+ requireNonNull(channel, "AnalogInput supplied to Gyro constructor is null");
+
+ m_analog = channel;
+ initGyro();
+ AnalogGyroJNI.setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
+ offset, center);
+ reset();
+ }
+
+ @Override
+ public void reset() {
+ AnalogGyroJNI.resetAnalogGyro(m_gyroHandle);
+ }
+
+ /**
+ * Delete (free) the accumulator and the analog components used for the gyro.
+ */
+ @Override
+ public void close() {
+ super.close();
+ if (m_analog != null && m_channelAllocated) {
+ m_analog.close();
+ }
+ m_analog = null;
+ AnalogGyroJNI.freeAnalogGyro(m_gyroHandle);
+ }
+
+ @Override
+ public double getAngle() {
+ if (m_analog == null) {
+ return 0.0;
+ } else {
+ return AnalogGyroJNI.getAnalogGyroAngle(m_gyroHandle);
+ }
+ }
+
+ @Override
+ public double getRate() {
+ if (m_analog == null) {
+ return 0.0;
+ } else {
+ return AnalogGyroJNI.getAnalogGyroRate(m_gyroHandle);
+ }
+ }
+
+ /**
+ * Return the gyro offset value set during calibration to use as a future preset.
+ *
+ * @return the current offset value
+ */
+ public double getOffset() {
+ return AnalogGyroJNI.getAnalogGyroOffset(m_gyroHandle);
+ }
+
+ /**
+ * Return the gyro center value set during calibration to use as a future preset.
+ *
+ * @return the current center value
+ */
+ public int getCenter() {
+ return AnalogGyroJNI.getAnalogGyroCenter(m_gyroHandle);
+ }
+
+ /**
+ * Set the gyro sensitivity. This takes the number of volts/degree/second sensitivity of the gyro
+ * and uses it in subsequent calculations to allow the code to work with multiple gyros. This
+ * value is typically found in the gyro datasheet.
+ *
+ * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
+ */
+ public void setSensitivity(double voltsPerDegreePerSecond) {
+ AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
+ voltsPerDegreePerSecond);
+ }
+
+ /**
+ * Set the size of the neutral zone. Any voltage from the gyro less than this amount from the
+ * center is considered stationary. Setting a deadband will decrease the amount of drift when the
+ * gyro isn't rotating, but will make it less accurate.
+ *
+ * @param volts The size of the deadband in volts
+ */
+ void setDeadband(double volts) {
+ AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, volts);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
new file mode 100644
index 0000000..74eb866
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -0,0 +1,356 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AccumulatorResult;
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogInSim;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Analog channel class.
+ *
+ * <p>Each analog channel is read from hardware as a 12-bit number representing 0V to 5V.
+ *
+ * <p>Connected to each analog channel is an averaging and oversampling engine. This engine
+ * accumulates the specified ( by setAverageBits() and setOversampleBits() ) number of samples
+ * before returning a new value. This is not a sliding window average. The only difference between
+ * the oversampled samples and the averaged samples is that the oversampled samples are simply
+ * accumulated effectively increasing the resolution, while the averaged samples are divided by the
+ * number of samples to retain the resolution, but get more stable values.
+ */
+public class AnalogInput extends SendableBase implements PIDSource {
+ private static final int kAccumulatorSlot = 1;
+ int m_port; // explicit no modifier, private and package accessible.
+ private int m_channel;
+ private static final int[] kAccumulatorChannels = {0, 1};
+ private long m_accumulatorOffset;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Construct an analog channel.
+ *
+ * @param channel The channel number to represent. 0-3 are on-board 4-7 are on the MXP port.
+ */
+ public AnalogInput(final int channel) {
+ AnalogJNI.checkAnalogInputChannel(channel);
+ m_channel = channel;
+
+ final int portHandle = HAL.getPort((byte) channel);
+ m_port = AnalogJNI.initializeAnalogInputPort(portHandle);
+
+ HAL.report(tResourceType.kResourceType_AnalogChannel, channel);
+ setName("AnalogInput", channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ AnalogJNI.freeAnalogInputPort(m_port);
+ m_port = 0;
+ m_channel = 0;
+ m_accumulatorOffset = 0;
+ }
+
+ /**
+ * Get a sample straight from this channel. The sample is a 12-bit value representing the 0V to 5V
+ * range of the A/D converter. The units are in A/D converter codes. Use GetVoltage() to get the
+ * analog value in calibrated units.
+ *
+ * @return A sample straight from this channel.
+ */
+ public int getValue() {
+ return AnalogJNI.getAnalogValue(m_port);
+ }
+
+ /**
+ * Get a sample from the output of the oversample and average engine for this channel. The sample
+ * is 12-bit + the bits configured in SetOversampleBits(). The value configured in
+ * setAverageBits() will cause this value to be averaged 2^bits number of samples. This is not a
+ * sliding window. The sample will not change until 2^(OversampleBits + AverageBits) samples have
+ * been acquired from this channel. Use getAverageVoltage() to get the analog value in calibrated
+ * units.
+ *
+ * @return A sample from the oversample and average engine for this channel.
+ */
+ public int getAverageValue() {
+ return AnalogJNI.getAnalogAverageValue(m_port);
+ }
+
+ /**
+ * Get a scaled sample straight from this channel. The value is scaled to units of Volts using the
+ * calibrated scaling data from getLSBWeight() and getOffset().
+ *
+ * @return A scaled sample straight from this channel.
+ */
+ public double getVoltage() {
+ return AnalogJNI.getAnalogVoltage(m_port);
+ }
+
+ /**
+ * Get a scaled sample from the output of the oversample and average engine for this channel. The
+ * value is scaled to units of Volts using the calibrated scaling data from getLSBWeight() and
+ * getOffset(). Using oversampling will cause this value to be higher resolution, but it will
+ * update more slowly. Using averaging will cause this value to be more stable, but it will update
+ * more slowly.
+ *
+ * @return A scaled sample from the output of the oversample and average engine for this channel.
+ */
+ public double getAverageVoltage() {
+ return AnalogJNI.getAnalogAverageVoltage(m_port);
+ }
+
+ /**
+ * Get the factory scaling least significant bit weight constant. The least significant bit weight
+ * constant for the channel that was calibrated in manufacturing and stored in an eeprom.
+ *
+ * <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @return Least significant bit weight.
+ */
+ public long getLSBWeight() {
+ return AnalogJNI.getAnalogLSBWeight(m_port);
+ }
+
+ /**
+ * Get the factory scaling offset constant. The offset constant for the channel that was
+ * calibrated in manufacturing and stored in an eeprom.
+ *
+ * <p>Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @return Offset constant.
+ */
+ public int getOffset() {
+ return AnalogJNI.getAnalogOffset(m_port);
+ }
+
+ /**
+ * Get the channel number.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Set the number of averaging bits. This sets the number of averaging bits. The actual number of
+ * averaged samples is 2^bits. The averaging is done automatically in the FPGA.
+ *
+ * @param bits The number of averaging bits.
+ */
+ public void setAverageBits(final int bits) {
+ AnalogJNI.setAnalogAverageBits(m_port, bits);
+ }
+
+ /**
+ * Get the number of averaging bits. This gets the number of averaging bits from the FPGA. The
+ * actual number of averaged samples is 2^bits. The averaging is done automatically in the FPGA.
+ *
+ * @return The number of averaging bits.
+ */
+ public int getAverageBits() {
+ return AnalogJNI.getAnalogAverageBits(m_port);
+ }
+
+ /**
+ * Set the number of oversample bits. This sets the number of oversample bits. The actual number
+ * of oversampled values is 2^bits. The oversampling is done automatically in the FPGA.
+ *
+ * @param bits The number of oversample bits.
+ */
+ public void setOversampleBits(final int bits) {
+ AnalogJNI.setAnalogOversampleBits(m_port, bits);
+ }
+
+ /**
+ * Get the number of oversample bits. This gets the number of oversample bits from the FPGA. The
+ * actual number of oversampled values is 2^bits. The oversampling is done automatically in the
+ * FPGA.
+ *
+ * @return The number of oversample bits.
+ */
+ public int getOversampleBits() {
+ return AnalogJNI.getAnalogOversampleBits(m_port);
+ }
+
+ /**
+ * Initialize the accumulator.
+ */
+ public void initAccumulator() {
+ if (!isAccumulatorChannel()) {
+ throw new AllocationException("Accumulators are only available on slot " + kAccumulatorSlot
+ + " on channels " + kAccumulatorChannels[0] + ", " + kAccumulatorChannels[1]);
+ }
+ m_accumulatorOffset = 0;
+ AnalogJNI.initAccumulator(m_port);
+ }
+
+ /**
+ * Set an initial value for the accumulator.
+ *
+ * <p>This will be added to all values returned to the user.
+ *
+ * @param initialValue The value that the accumulator should start from when reset.
+ */
+ public void setAccumulatorInitialValue(long initialValue) {
+ m_accumulatorOffset = initialValue;
+ }
+
+ /**
+ * Resets the accumulator to the initial value.
+ */
+ public void resetAccumulator() {
+ AnalogJNI.resetAccumulator(m_port);
+
+ // Wait until the next sample, so the next call to getAccumulator*()
+ // won't have old values.
+ final double sampleTime = 1.0 / getGlobalSampleRate();
+ final double overSamples = 1 << getOversampleBits();
+ final double averageSamples = 1 << getAverageBits();
+ Timer.delay(sampleTime * overSamples * averageSamples);
+
+ }
+
+ /**
+ * Set the center value of the accumulator.
+ *
+ * <p>The center value is subtracted from each A/D value before it is added to the accumulator.
+ * This is used for the center value of devices like gyros and accelerometers to take the device
+ * offset into account when integrating.
+ *
+ * <p>This center value is based on the output of the oversampled and averaged source the
+ * accumulator channel. Because of this, any non-zero oversample bits will affect the size of the
+ * value for this field.
+ */
+ public void setAccumulatorCenter(int center) {
+ AnalogJNI.setAccumulatorCenter(m_port, center);
+ }
+
+ /**
+ * Set the accumulator's deadband.
+ *
+ * @param deadband The deadband size in ADC codes (12-bit value)
+ */
+ public void setAccumulatorDeadband(int deadband) {
+ AnalogJNI.setAccumulatorDeadband(m_port, deadband);
+ }
+
+ /**
+ * Read the accumulated value.
+ *
+ * <p>Read the value that has been accumulating. The accumulator is attached after the oversample
+ * and average engine.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+ public long getAccumulatorValue() {
+ return AnalogJNI.getAccumulatorValue(m_port) + m_accumulatorOffset;
+ }
+
+ /**
+ * Read the number of accumulated values.
+ *
+ * <p>Read the count of the accumulated values since the accumulator was last Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+ public long getAccumulatorCount() {
+ return AnalogJNI.getAccumulatorCount(m_port);
+ }
+
+ /**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * <p>This function reads the value and count from the FPGA atomically. This can be used for
+ * averaging.
+ *
+ * @param result AccumulatorResult object to store the results in.
+ */
+ public void getAccumulatorOutput(AccumulatorResult result) {
+ if (result == null) {
+ throw new IllegalArgumentException("Null parameter `result'");
+ }
+ if (!isAccumulatorChannel()) {
+ throw new IllegalArgumentException(
+ "Channel " + m_channel + " is not an accumulator channel.");
+ }
+ AnalogJNI.getAccumulatorOutput(m_port, result);
+ result.value += m_accumulatorOffset;
+ }
+
+ /**
+ * Is the channel attached to an accumulator.
+ *
+ * @return The analog channel is attached to an accumulator.
+ */
+ public boolean isAccumulatorChannel() {
+ for (int channel : kAccumulatorChannels) {
+ if (m_channel == channel) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Set the sample rate per channel.
+ *
+ * <p>This is a global setting for all channels. The maximum rate is 500kS/s divided by the number
+ * of channels in use. This is 62500 samples/s per channel if all 8 channels are used.
+ *
+ * @param samplesPerSecond The number of samples per second.
+ */
+ public static void setGlobalSampleRate(final double samplesPerSecond) {
+ AnalogJNI.setAnalogSampleRate(samplesPerSecond);
+ }
+
+ /**
+ * Get the current sample rate.
+ *
+ * <p>This assumes one entry in the scan list. This is a global setting for all channels.
+ *
+ * @return Sample rate.
+ */
+ public static double getGlobalSampleRate() {
+ return AnalogJNI.getAnalogSampleRate();
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the average voltage for use with PIDController.
+ *
+ * @return the average voltage
+ */
+ @Override
+ public double pidGet() {
+ return getAverageVoltage();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Analog Input");
+ builder.addDoubleProperty("Value", this::getAverageVoltage, null);
+ }
+
+ public AnalogInSim getSimObject() {
+ return new AnalogInSim(m_channel);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
new file mode 100644
index 0000000..ff81e9e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogOutSim;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Analog output class.
+ */
+public class AnalogOutput extends SendableBase {
+ private int m_port;
+ private int m_channel;
+
+ /**
+ * Construct an analog output on a specified MXP channel.
+ *
+ * @param channel The channel number to represent.
+ */
+ public AnalogOutput(final int channel) {
+ SensorUtil.checkAnalogOutputChannel(channel);
+ m_channel = channel;
+
+ final int portHandle = HAL.getPort((byte) channel);
+ m_port = AnalogJNI.initializeAnalogOutputPort(portHandle);
+
+ HAL.report(tResourceType.kResourceType_AnalogOutput, channel);
+ setName("AnalogOutput", channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ AnalogJNI.freeAnalogOutputPort(m_port);
+ m_port = 0;
+ m_channel = 0;
+ }
+
+ /**
+ * Get the channel of this AnalogOutput.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ public void setVoltage(double voltage) {
+ AnalogJNI.setAnalogOutput(m_port, voltage);
+ }
+
+ public double getVoltage() {
+ return AnalogJNI.getAnalogOutput(m_port);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Analog Output");
+ builder.addDoubleProperty("Value", this::getVoltage, this::setVoltage);
+ }
+
+ public AnalogOutSim getSimObject() {
+ return new AnalogOutSim(m_channel);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
new file mode 100644
index 0000000..d1a9018
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.interfaces.Potentiometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that
+ * corresponds to a position. The position is in whichever units you choose, by way of the scaling
+ * and offset constants passed to the constructor.
+ */
+public class AnalogPotentiometer extends SendableBase implements Potentiometer {
+ private AnalogInput m_analogInput;
+ private boolean m_initAnalogInput;
+ private double m_fullRange;
+ private double m_offset;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees. This will calculate the result from the fullRange times
+ * the fraction of the supply voltage, plus the offset.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
+ * @param offset The offset to add to the scaled value for controlling the zero value
+ */
+ public AnalogPotentiometer(final int channel, double fullRange, double offset) {
+ this(new AnalogInput(channel), fullRange, offset);
+ m_initAnalogInput = true;
+ addChild(m_analogInput);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees. This will calculate the result from the fullRange times
+ * the fraction of the supply voltage, plus the offset.
+ *
+ * @param input The {@link AnalogInput} this potentiometer is plugged into.
+ * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
+ * @param offset The offset to add to the scaled value for controlling the zero value
+ */
+ public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
+ setName("AnalogPotentiometer", input.getChannel());
+ m_analogInput = input;
+ m_initAnalogInput = false;
+
+ m_fullRange = fullRange;
+ m_offset = offset;
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+ */
+ public AnalogPotentiometer(final int channel, double scale) {
+ this(channel, scale, 0);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
+ * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+ * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+ * point after scaling is 135 degrees.
+ *
+ * @param input The {@link AnalogInput} this potentiometer is plugged into.
+ * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+ */
+ public AnalogPotentiometer(final AnalogInput input, double scale) {
+ this(input, scale, 0);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ */
+ public AnalogPotentiometer(final int channel) {
+ this(channel, 1, 0);
+ }
+
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * @param input The {@link AnalogInput} this potentiometer is plugged into.
+ */
+ public AnalogPotentiometer(final AnalogInput input) {
+ this(input, 1, 0);
+ }
+
+ /**
+ * Get the current reading of the potentiometer.
+ *
+ * @return The current position of the potentiometer.
+ */
+ @Override
+ public double get() {
+ if (m_analogInput == null) {
+ return m_offset;
+ }
+ return (m_analogInput.getVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+ throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
+ }
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+ @Override
+ public double pidGet() {
+ return get();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ if (m_analogInput != null) {
+ m_analogInput.initSendable(builder);
+ }
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ if (m_initAnalogInput) {
+ m_analogInput.close();
+ m_analogInput = null;
+ m_initAnalogInput = false;
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
new file mode 100644
index 0000000..e429246
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -0,0 +1,184 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.BoundaryException;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+
+/**
+ * Class for creating and configuring Analog Triggers.
+ */
+public class AnalogTrigger extends SendableBase {
+ /**
+ * Exceptions dealing with improper operation of the Analog trigger.
+ */
+ public static class AnalogTriggerException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ public AnalogTriggerException(String message) {
+ super(message);
+ }
+
+ }
+
+ /**
+ * Where the analog trigger is attached.
+ */
+ protected int m_port;
+ protected int m_index;
+ protected AnalogInput m_analogInput;
+ protected boolean m_ownsAnalog;
+
+ /**
+ * Constructor for an analog trigger given a channel number.
+ *
+ * @param channel the port to use for the analog trigger
+ */
+ public AnalogTrigger(final int channel) {
+ this(new AnalogInput(channel));
+ m_ownsAnalog = true;
+ addChild(m_analogInput);
+ }
+
+ /**
+ * Construct an analog trigger given an analog channel. This should be used in the case of sharing
+ * an analog channel between the trigger and an analog input object.
+ *
+ * @param channel the AnalogInput to use for the analog trigger
+ */
+ public AnalogTrigger(AnalogInput channel) {
+ m_analogInput = channel;
+ ByteBuffer index = ByteBuffer.allocateDirect(4);
+ index.order(ByteOrder.LITTLE_ENDIAN);
+
+ m_port =
+ AnalogJNI.initializeAnalogTrigger(channel.m_port, index.asIntBuffer());
+ m_index = index.asIntBuffer().get(0);
+
+ HAL.report(tResourceType.kResourceType_AnalogTrigger, channel.getChannel());
+ setName("AnalogTrigger", channel.getChannel());
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ AnalogJNI.cleanAnalogTrigger(m_port);
+ m_port = 0;
+ if (m_ownsAnalog && m_analogInput != null) {
+ m_analogInput.close();
+ }
+ }
+
+ /**
+ * Set the upper and lower limits of the analog trigger. The limits are given in ADC codes. If
+ * oversampling is used, the units must be scaled appropriately.
+ *
+ * @param lower the lower raw limit
+ * @param upper the upper raw limit
+ */
+ public void setLimitsRaw(final int lower, final int upper) {
+ if (lower > upper) {
+ throw new BoundaryException("Lower bound is greater than upper");
+ }
+ AnalogJNI.setAnalogTriggerLimitsRaw(m_port, lower, upper);
+ }
+
+ /**
+ * Set the upper and lower limits of the analog trigger. The limits are given as floating point
+ * voltage values.
+ *
+ * @param lower the lower voltage limit
+ * @param upper the upper voltage limit
+ */
+ public void setLimitsVoltage(double lower, double upper) {
+ if (lower > upper) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ AnalogJNI.setAnalogTriggerLimitsVoltage(m_port, lower, upper);
+ }
+
+ /**
+ * Configure the analog trigger to use the averaged vs. raw values. If the value is true, then the
+ * averaged value is selected for the analog trigger, otherwise the immediate value is used.
+ *
+ * @param useAveragedValue true to use an averaged value, false otherwise
+ */
+ public void setAveraged(boolean useAveragedValue) {
+ AnalogJNI.setAnalogTriggerAveraged(m_port, useAveragedValue);
+ }
+
+ /**
+ * Configure the analog trigger to use a filtered value. The analog trigger will operate with a 3
+ * point average rejection filter. This is designed to help with 360 degree pot applications for
+ * the period where the pot crosses through zero.
+ *
+ * @param useFilteredValue true to use a filtered value, false otherwise
+ */
+ public void setFiltered(boolean useFilteredValue) {
+ AnalogJNI.setAnalogTriggerFiltered(m_port, useFilteredValue);
+ }
+
+ /**
+ * Return the index of the analog trigger. This is the FPGA index of this analog trigger
+ * instance.
+ *
+ * @return The index of the analog trigger.
+ */
+ public int getIndex() {
+ return m_index;
+ }
+
+ /**
+ * Return the InWindow output of the analog trigger. True if the analog input is between the upper
+ * and lower limits.
+ *
+ * @return The InWindow output of the analog trigger.
+ */
+ public boolean getInWindow() {
+ return AnalogJNI.getAnalogTriggerInWindow(m_port);
+ }
+
+ /**
+ * Return the TriggerState output of the analog trigger. True if above upper limit. False if below
+ * lower limit. If in Hysteresis, maintain previous state.
+ *
+ * @return The TriggerState output of the analog trigger.
+ */
+ public boolean getTriggerState() {
+ return AnalogJNI.getAnalogTriggerTriggerState(m_port);
+ }
+
+ /**
+ * Creates an AnalogTriggerOutput object. Gets an output object that can be used for routing.
+ * Caller is responsible for deleting the AnalogTriggerOutput object.
+ *
+ * @param type An enum of the type of output object to create.
+ * @return A pointer to a new AnalogTriggerOutput object.
+ */
+ public AnalogTriggerOutput createOutput(AnalogTriggerType type) {
+ return new AnalogTriggerOutput(this, type);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ if (m_ownsAnalog) {
+ m_analogInput.initSendable(builder);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
new file mode 100644
index 0000000..04971c8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Class to represent a specific output from an analog trigger. This class is used to get the
+ * current output value and also as a DigitalSource to provide routing of an output to digital
+ * subsystems on the FPGA such as Counter, Encoder, and Interrupt.
+ *
+ * <p>The TriggerState output indicates the primary output value of the trigger. If the analog
+ * signal is less than the lower limit, the output is false. If the analog value is greater than the
+ * upper limit, then the output is true. If the analog value is in between, then the trigger output
+ * state maintains its most recent value.
+ *
+ * <p>The InWindow output indicates whether or not the analog signal is inside the range defined by
+ * the limits.
+ *
+ * <p>The RisingPulse and FallingPulse outputs detect an instantaneous transition from above the
+ * upper limit to below the lower limit, and vise versa. These pulses represent a rollover condition
+ * of a sensor and can be routed to an up / down counter or to interrupts. Because the outputs
+ * generate a pulse, they cannot be read directly. To help ensure that a rollover condition is not
+ * missed, there is an average rejection filter available that operates on the upper 8 bits of a 12
+ * bit number and selects the nearest outlier of 3 samples. This will reject a sample that is (due
+ * to averaging or sampling) errantly between the two limits. This filter will fail if more than one
+ * sample in a row is errantly in between the two limits. You may see this problem if attempting to
+ * use this feature with a mechanical rollover sensor, such as a 360 degree no-stop potentiometer
+ * without signal conditioning, because the rollover transition is not sharp / clean enough. Using
+ * the averaging engine may help with this, but rotational speeds of the sensor will then be
+ * limited.
+ */
+public class AnalogTriggerOutput extends DigitalSource {
+ /**
+ * Exceptions dealing with improper operation of the Analog trigger output.
+ */
+ public static class AnalogTriggerOutputException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ public AnalogTriggerOutputException(String message) {
+ super(message);
+ }
+ }
+
+ private final AnalogTrigger m_trigger;
+ private final AnalogTriggerType m_outputType;
+
+ /**
+ * Create an object that represents one of the four outputs from an analog trigger.
+ *
+ * <p>Because this class derives from DigitalSource, it can be passed into routing functions for
+ * Counter, Encoder, etc.
+ *
+ * @param trigger The trigger for which this is an output.
+ * @param outputType An enum that specifies the output on the trigger to represent.
+ */
+ public AnalogTriggerOutput(AnalogTrigger trigger, final AnalogTriggerType outputType) {
+ requireNonNull(trigger, "Analog Trigger given was null");
+ requireNonNull(outputType, "Analog Trigger Type given was null");
+
+ m_trigger = trigger;
+ m_outputType = outputType;
+ HAL.report(tResourceType.kResourceType_AnalogTriggerOutput,
+ trigger.getIndex(), outputType.value);
+ }
+
+ /**
+ * Get the state of the analog trigger output.
+ *
+ * @return The state of the analog trigger output.
+ */
+ public boolean get() {
+ return AnalogJNI.getAnalogTriggerOutput(m_trigger.m_port, m_outputType.value);
+ }
+
+ @Override
+ public int getPortHandleForRouting() {
+ return m_trigger.m_port;
+ }
+
+ @Override
+ public int getAnalogTriggerTypeForRouting() {
+ return m_outputType.value;
+ }
+
+ @Override
+ public int getChannel() {
+ return m_trigger.m_index;
+ }
+
+ @Override
+ public boolean isAnalogTrigger() {
+ return true;
+ }
+
+ /**
+ * Defines the state in which the AnalogTrigger triggers.
+ */
+ public enum AnalogTriggerType {
+ kInWindow(AnalogJNI.AnalogTriggerType.kInWindow), kState(AnalogJNI.AnalogTriggerType.kState),
+ kRisingPulse(AnalogJNI.AnalogTriggerType.kRisingPulse),
+ kFallingPulse(AnalogJNI.AnalogTriggerType.kFallingPulse);
+
+ @SuppressWarnings("MemberName")
+ private final int value;
+
+ AnalogTriggerType(int value) {
+ this.value = value;
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
new file mode 100644
index 0000000..d8dbafe
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AccelerometerJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AccelerometerSim;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Built-in accelerometer.
+ *
+ * <p>This class allows access to the roboRIO's internal accelerometer.
+ */
+public class BuiltInAccelerometer extends SendableBase implements Accelerometer {
+ /**
+ * Constructor.
+ *
+ * @param range The range the accelerometer will measure
+ */
+ public BuiltInAccelerometer(Range range) {
+ setRange(range);
+ HAL.report(tResourceType.kResourceType_Accelerometer, 0, 0, "Built-in accelerometer");
+ setName("BuiltInAccel", 0);
+ }
+
+ /**
+ * Constructor. The accelerometer will measure +/-8 g-forces
+ */
+ public BuiltInAccelerometer() {
+ this(Range.k8G);
+ }
+
+ @Override
+ public void setRange(Range range) {
+ AccelerometerJNI.setAccelerometerActive(false);
+
+ switch (range) {
+ case k2G:
+ AccelerometerJNI.setAccelerometerRange(0);
+ break;
+ case k4G:
+ AccelerometerJNI.setAccelerometerRange(1);
+ break;
+ case k8G:
+ AccelerometerJNI.setAccelerometerRange(2);
+ break;
+ case k16G:
+ default:
+ throw new IllegalArgumentException(range + " range not supported (use k2G, k4G, or k8G)");
+
+ }
+
+ AccelerometerJNI.setAccelerometerActive(true);
+ }
+
+ /**
+ * The acceleration in the X axis.
+ *
+ * @return The acceleration of the roboRIO along the X axis in g-forces
+ */
+ @Override
+ public double getX() {
+ return AccelerometerJNI.getAccelerometerX();
+ }
+
+ /**
+ * The acceleration in the Y axis.
+ *
+ * @return The acceleration of the roboRIO along the Y axis in g-forces
+ */
+ @Override
+ public double getY() {
+ return AccelerometerJNI.getAccelerometerY();
+ }
+
+ /**
+ * The acceleration in the Z axis.
+ *
+ * @return The acceleration of the roboRIO along the Z axis in g-forces
+ */
+ @Override
+ public double getZ() {
+ return AccelerometerJNI.getAccelerometerZ();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("3AxisAccelerometer");
+ builder.addDoubleProperty("X", this::getX, null);
+ builder.addDoubleProperty("Y", this::getY, null);
+ builder.addDoubleProperty("Z", this::getZ, null);
+ }
+
+ public AccelerometerSim getSimObject() {
+ return new AccelerometerSim();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
new file mode 100644
index 0000000..c6eac3a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.Closeable;
+
+import edu.wpi.first.hal.CANAPIJNI;
+import edu.wpi.first.hal.CANData;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * High level class for interfacing with CAN devices conforming to
+ * the standard CAN spec.
+ *
+ * <p>No packets that can be sent gets blocked by the RoboRIO, so all methods
+ * work identically in all robot modes.
+ *
+ * <p>All methods are thread safe, however the CANData object passed into the
+ * read methods and the byte[] passed into the write methods need to not
+ * be modified for the duration of their respective calls.
+ */
+public class CAN implements Closeable {
+ public static final int kTeamManufacturer = 8;
+ public static final int kTeamDeviceType = 10;
+
+ private final int m_handle;
+
+ /**
+ * Create a new CAN communication interface with the specific device ID.
+ * This uses the team manufacturer and device types.
+ * The device ID is 6 bits (0-63).
+ *
+ * @param deviceId The device id
+ */
+ public CAN(int deviceId) {
+ m_handle = CANAPIJNI.initializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType);
+ HAL.report(tResourceType.kResourceType_CAN, deviceId);
+ }
+
+ /**
+ * Create a new CAN communication interface with a specific device ID,
+ * manufacturer and device type. The device ID is 6 bits, the
+ * manufacturer is 8 bits, and the device type is 5 bits.
+ *
+ * @param deviceId The device ID
+ * @param deviceManufacturer The device manufacturer
+ * @param deviceType The device type
+ */
+ public CAN(int deviceId, int deviceManufacturer, int deviceType) {
+ m_handle = CANAPIJNI.initializeCAN(deviceManufacturer, deviceId, deviceType);
+ HAL.report(tResourceType.kResourceType_CAN, deviceId);
+ }
+
+ /**
+ * Closes the CAN communication.
+ */
+ @Override
+ public void close() {
+ if (m_handle != 0) {
+ CANAPIJNI.cleanCAN(m_handle);
+ }
+ }
+
+ /**
+ * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+ *
+ * @param data The data to write (8 bytes max)
+ * @param apiId The API ID to write.
+ */
+ public void writePacket(byte[] data, int apiId) {
+ CANAPIJNI.writeCANPacket(m_handle, data, apiId);
+ }
+
+ /**
+ * Write a repeating packet to the CAN device with a specific ID. This ID is 10 bits.
+ * The RoboRIO will automatically repeat the packet at the specified interval
+ *
+ * @param data The data to write (8 bytes max)
+ * @param apiId The API ID to write.
+ * @param repeatMs The period to repeat the packet at.
+ */
+ public void writePacketRepeating(byte[] data, int apiId, int repeatMs) {
+ CANAPIJNI.writeCANPacketRepeating(m_handle, data, apiId, repeatMs);
+ }
+
+ /**
+ * Stop a repeating packet with a specific ID. This ID is 10 bits.
+ *
+ * @param apiId The API ID to stop repeating
+ */
+ public void stopPacketRepeating(int apiId) {
+ CANAPIJNI.stopCANPacketRepeating(m_handle, apiId);
+ }
+
+ /**
+ * Read a new CAN packet. This will only return properly once per packet
+ * received. Multiple calls without receiving another packet will return false.
+ *
+ * @param apiId The API ID to read.
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPacketNew(int apiId, CANData data) {
+ return CANAPIJNI.readCANPacketNew(m_handle, apiId, data);
+ }
+
+ /**
+ * Read a CAN packet. The will continuously return the last packet received,
+ * without accounting for packet age.
+ *
+ * @param apiId The API ID to read.
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPacketLatest(int apiId, CANData data) {
+ return CANAPIJNI.readCANPacketLatest(m_handle, apiId, data);
+ }
+
+ /**
+ * Read a CAN packet. The will return the last packet received until the
+ * packet is older then the requested timeout. Then it will return false.
+ *
+ * @param apiId The API ID to read.
+ * @param timeoutMs The timeout time for the packet
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPacketTimeout(int apiId, int timeoutMs, CANData data) {
+ return CANAPIJNI.readCANPacketTimeout(m_handle, apiId, timeoutMs, data);
+ }
+
+ /**
+ * Read a CAN packet. The will return the last packet received until the
+ * packet is older then the requested timeout. Then it will return false.
+ * The period parameter is used when you know the packet is sent at specific
+ * intervals, so calls will not attempt to read a new packet from the
+ * network until that period has passed. We do not recommend users use this
+ * API unless they know the implications.
+ *
+ * @param apiId The API ID to read.
+ * @param timeoutMs The timeout time for the packet
+ * @param periodMs The usual period for the packet
+ * @param data Storage for the received data.
+ * @return True if the data is valid, otherwise false.
+ */
+ public boolean readPeriodicPacket(int apiId, int timeoutMs, int periodMs, CANData data) {
+ return CANAPIJNI.readCANPeriodicPacket(m_handle, apiId, timeoutMs, periodMs, data);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
new file mode 100644
index 0000000..a25ab2e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
@@ -0,0 +1,780 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Hashtable;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+
+import edu.wpi.cscore.AxisCamera;
+import edu.wpi.cscore.CameraServerJNI;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.MjpegServer;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoEvent;
+import edu.wpi.cscore.VideoException;
+import edu.wpi.cscore.VideoListener;
+import edu.wpi.cscore.VideoMode;
+import edu.wpi.cscore.VideoMode.PixelFormat;
+import edu.wpi.cscore.VideoProperty;
+import edu.wpi.cscore.VideoSink;
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * Singleton class for creating and keeping camera servers.
+ * Also publishes camera information to NetworkTables.
+ *
+ * @deprecated Replaced with edu.wpi.first.cameraserver.CameraServer
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+@Deprecated
+public final class CameraServer {
+ public static final int kBasePort = 1181;
+
+ @Deprecated
+ public static final int kSize640x480 = 0;
+ @Deprecated
+ public static final int kSize320x240 = 1;
+ @Deprecated
+ public static final int kSize160x120 = 2;
+
+ private static final String kPublishName = "/CameraPublisher";
+ private static CameraServer server;
+
+ /**
+ * Get the CameraServer instance.
+ */
+ public static synchronized CameraServer getInstance() {
+ if (server == null) {
+ server = new CameraServer();
+ }
+ return server;
+ }
+
+ private final AtomicInteger m_defaultUsbDevice;
+ private String m_primarySourceName;
+ private final Map<String, VideoSource> m_sources;
+ private final Map<String, VideoSink> m_sinks;
+ private final Map<Integer, NetworkTable> m_tables; // indexed by source handle
+ private final NetworkTable m_publishTable;
+ private final VideoListener m_videoListener; //NOPMD
+ private final int m_tableListener; //NOPMD
+ private int m_nextPort;
+ private String[] m_addresses;
+
+ @SuppressWarnings("JavadocMethod")
+ private static String makeSourceValue(int source) {
+ switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
+ case kUsb:
+ return "usb:" + CameraServerJNI.getUsbCameraPath(source);
+ case kHttp: {
+ String[] urls = CameraServerJNI.getHttpCameraUrls(source);
+ if (urls.length > 0) {
+ return "ip:" + urls[0];
+ } else {
+ return "ip:";
+ }
+ }
+ case kCv:
+ // FIXME: Should be "cv:", but LabVIEW dashboard requires "usb:".
+ // https://github.com/wpilibsuite/allwpilib/issues/407
+ return "usb:";
+ default:
+ return "unknown:";
+ }
+ }
+
+ @SuppressWarnings("JavadocMethod")
+ private static String makeStreamValue(String address, int port) {
+ return "mjpg:http://" + address + ":" + port + "/?action=stream";
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+ private synchronized String[] getSinkStreamValues(int sink) {
+ // Ignore all but MjpegServer
+ if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
+ return new String[0];
+ }
+
+ // Get port
+ int port = CameraServerJNI.getMjpegServerPort(sink);
+
+ // Generate values
+ ArrayList<String> values = new ArrayList<>(m_addresses.length + 1);
+ String listenAddress = CameraServerJNI.getMjpegServerListenAddress(sink);
+ if (!listenAddress.isEmpty()) {
+ // If a listen address is specified, only use that
+ values.add(makeStreamValue(listenAddress, port));
+ } else {
+ // Otherwise generate for hostname and all interface addresses
+ values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
+ for (String addr : m_addresses) {
+ if ("127.0.0.1".equals(addr)) {
+ continue; // ignore localhost
+ }
+ values.add(makeStreamValue(addr, port));
+ }
+ }
+
+ return values.toArray(new String[0]);
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+ private synchronized String[] getSourceStreamValues(int source) {
+ // Ignore all but HttpCamera
+ if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
+ != VideoSource.Kind.kHttp) {
+ return new String[0];
+ }
+
+ // Generate values
+ String[] values = CameraServerJNI.getHttpCameraUrls(source);
+ for (int j = 0; j < values.length; j++) {
+ values[j] = "mjpg:" + values[j];
+ }
+
+ // Look to see if we have a passthrough server for this source
+ for (VideoSink i : m_sinks.values()) {
+ int sink = i.getHandle();
+ int sinkSource = CameraServerJNI.getSinkSource(sink);
+ if (source == sinkSource
+ && VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) == VideoSink.Kind.kMjpeg) {
+ // Add USB-only passthrough
+ String[] finalValues = Arrays.copyOf(values, values.length + 1);
+ int port = CameraServerJNI.getMjpegServerPort(sink);
+ finalValues[values.length] = makeStreamValue("172.22.11.2", port);
+ return finalValues;
+ }
+ }
+
+ return values;
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+ private synchronized void updateStreamValues() {
+ // Over all the sinks...
+ for (VideoSink i : m_sinks.values()) {
+ int sink = i.getHandle();
+
+ // Get the source's subtable (if none exists, we're done)
+ int source = CameraServerJNI.getSinkSource(sink);
+ if (source == 0) {
+ continue;
+ }
+ NetworkTable table = m_tables.get(source);
+ if (table != null) {
+ // Don't set stream values if this is a HttpCamera passthrough
+ if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
+ == VideoSource.Kind.kHttp) {
+ continue;
+ }
+
+ // Set table value
+ String[] values = getSinkStreamValues(sink);
+ if (values.length > 0) {
+ table.getEntry("streams").setStringArray(values);
+ }
+ }
+ }
+
+ // Over all the sources...
+ for (VideoSource i : m_sources.values()) {
+ int source = i.getHandle();
+
+ // Get the source's subtable (if none exists, we're done)
+ NetworkTable table = m_tables.get(source);
+ if (table != null) {
+ // Set table value
+ String[] values = getSourceStreamValues(source);
+ if (values.length > 0) {
+ table.getEntry("streams").setStringArray(values);
+ }
+ }
+ }
+ }
+
+ @SuppressWarnings("JavadocMethod")
+ private static String pixelFormatToString(PixelFormat pixelFormat) {
+ switch (pixelFormat) {
+ case kMJPEG:
+ return "MJPEG";
+ case kYUYV:
+ return "YUYV";
+ case kRGB565:
+ return "RGB565";
+ case kBGR:
+ return "BGR";
+ case kGray:
+ return "Gray";
+ default:
+ return "Unknown";
+ }
+ }
+
+ /// Provide string description of video mode.
+ /// The returned string is "{width}x{height} {format} {fps} fps".
+ @SuppressWarnings("JavadocMethod")
+ private static String videoModeToString(VideoMode mode) {
+ return mode.width + "x" + mode.height + " " + pixelFormatToString(mode.pixelFormat)
+ + " " + mode.fps + " fps";
+ }
+
+ @SuppressWarnings("JavadocMethod")
+ private static String[] getSourceModeValues(int sourceHandle) {
+ VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
+ String[] modeStrings = new String[modes.length];
+ for (int i = 0; i < modes.length; i++) {
+ modeStrings[i] = videoModeToString(modes[i]);
+ }
+ return modeStrings;
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.CyclomaticComplexity"})
+ private static void putSourcePropertyValue(NetworkTable table, VideoEvent event, boolean isNew) {
+ String name;
+ String infoName;
+ if (event.name.startsWith("raw_")) {
+ name = "RawProperty/" + event.name;
+ infoName = "RawPropertyInfo/" + event.name;
+ } else {
+ name = "Property/" + event.name;
+ infoName = "PropertyInfo/" + event.name;
+ }
+
+ NetworkTableEntry entry = table.getEntry(name);
+ try {
+ switch (event.propertyKind) {
+ case kBoolean:
+ if (isNew) {
+ entry.setDefaultBoolean(event.value != 0);
+ } else {
+ entry.setBoolean(event.value != 0);
+ }
+ break;
+ case kInteger:
+ case kEnum:
+ if (isNew) {
+ entry.setDefaultDouble(event.value);
+ table.getEntry(infoName + "/min").setDouble(
+ CameraServerJNI.getPropertyMin(event.propertyHandle));
+ table.getEntry(infoName + "/max").setDouble(
+ CameraServerJNI.getPropertyMax(event.propertyHandle));
+ table.getEntry(infoName + "/step").setDouble(
+ CameraServerJNI.getPropertyStep(event.propertyHandle));
+ table.getEntry(infoName + "/default").setDouble(
+ CameraServerJNI.getPropertyDefault(event.propertyHandle));
+ } else {
+ entry.setDouble(event.value);
+ }
+ break;
+ case kString:
+ if (isNew) {
+ entry.setDefaultString(event.valueStr);
+ } else {
+ entry.setString(event.valueStr);
+ }
+ break;
+ default:
+ break;
+ }
+ } catch (VideoException ignored) {
+ // ignore
+ }
+ }
+
+ @SuppressWarnings({"JavadocMethod", "PMD.UnusedLocalVariable", "PMD.ExcessiveMethodLength",
+ "PMD.NPathComplexity"})
+ private CameraServer() {
+ m_defaultUsbDevice = new AtomicInteger();
+ m_sources = new Hashtable<>();
+ m_sinks = new Hashtable<>();
+ m_tables = new Hashtable<>();
+ m_publishTable = NetworkTableInstance.getDefault().getTable(kPublishName);
+ m_nextPort = kBasePort;
+ m_addresses = new String[0];
+
+ // We publish sources to NetworkTables using the following structure:
+ // "/CameraPublisher/{Source.Name}/" - root
+ // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
+ // - "streams" (string array): URLs that can be used to stream data
+ // - "description" (string): Description of the source
+ // - "connected" (boolean): Whether source is connected
+ // - "mode" (string): Current video mode
+ // - "modes" (string array): Available video modes
+ // - "Property/{Property}" - Property values
+ // - "PropertyInfo/{Property}" - Property supporting information
+
+ // Listener for video events
+ m_videoListener = new VideoListener(event -> {
+ switch (event.kind) {
+ case kSourceCreated: {
+ // Create subtable for the camera
+ NetworkTable table = m_publishTable.getSubTable(event.name);
+ m_tables.put(event.sourceHandle, table);
+ table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
+ table.getEntry("description").setString(
+ CameraServerJNI.getSourceDescription(event.sourceHandle));
+ table.getEntry("connected").setBoolean(
+ CameraServerJNI.isSourceConnected(event.sourceHandle));
+ table.getEntry("streams").setStringArray(getSourceStreamValues(event.sourceHandle));
+ try {
+ VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
+ table.getEntry("mode").setDefaultString(videoModeToString(mode));
+ table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
+ } catch (VideoException ignored) {
+ // Do nothing. Let the other event handlers update this if there is an error.
+ }
+ break;
+ }
+ case kSourceDestroyed: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("source").setString("");
+ table.getEntry("streams").setStringArray(new String[0]);
+ table.getEntry("modes").setStringArray(new String[0]);
+ }
+ break;
+ }
+ case kSourceConnected: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ // update the description too (as it may have changed)
+ table.getEntry("description").setString(
+ CameraServerJNI.getSourceDescription(event.sourceHandle));
+ table.getEntry("connected").setBoolean(true);
+ }
+ break;
+ }
+ case kSourceDisconnected: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("connected").setBoolean(false);
+ }
+ break;
+ }
+ case kSourceVideoModesUpdated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
+ }
+ break;
+ }
+ case kSourceVideoModeChanged: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ table.getEntry("mode").setString(videoModeToString(event.mode));
+ }
+ break;
+ }
+ case kSourcePropertyCreated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ putSourcePropertyValue(table, event, true);
+ }
+ break;
+ }
+ case kSourcePropertyValueUpdated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ putSourcePropertyValue(table, event, false);
+ }
+ break;
+ }
+ case kSourcePropertyChoicesUpdated: {
+ NetworkTable table = m_tables.get(event.sourceHandle);
+ if (table != null) {
+ try {
+ String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
+ table.getEntry("PropertyInfo/" + event.name + "/choices").setStringArray(choices);
+ } catch (VideoException ignored) {
+ // ignore
+ }
+ }
+ break;
+ }
+ case kSinkSourceChanged:
+ case kSinkCreated:
+ case kSinkDestroyed:
+ case kNetworkInterfacesChanged: {
+ m_addresses = CameraServerJNI.getNetworkInterfaces();
+ updateStreamValues();
+ break;
+ }
+ default:
+ break;
+ }
+ }, 0x4fff, true);
+
+ // Listener for NetworkTable events
+ // We don't currently support changing settings via NT due to
+ // synchronization issues, so just update to current setting if someone
+ // else tries to change it.
+ m_tableListener = NetworkTableInstance.getDefault().addEntryListener(kPublishName + "/",
+ event -> {
+ String relativeKey = event.name.substring(kPublishName.length() + 1);
+
+ // get source (sourceName/...)
+ int subKeyIndex = relativeKey.indexOf('/');
+ if (subKeyIndex == -1) {
+ return;
+ }
+ String sourceName = relativeKey.substring(0, subKeyIndex);
+ VideoSource source = m_sources.get(sourceName);
+ if (source == null) {
+ return;
+ }
+
+ // get subkey
+ relativeKey = relativeKey.substring(subKeyIndex + 1);
+
+ // handle standard names
+ String propName;
+ if ("mode".equals(relativeKey)) {
+ // reset to current mode
+ event.getEntry().setString(videoModeToString(source.getVideoMode()));
+ return;
+ } else if (relativeKey.startsWith("Property/")) {
+ propName = relativeKey.substring(9);
+ } else if (relativeKey.startsWith("RawProperty/")) {
+ propName = relativeKey.substring(12);
+ } else {
+ return; // ignore
+ }
+
+ // everything else is a property
+ VideoProperty property = source.getProperty(propName);
+ switch (property.getKind()) {
+ case kNone:
+ return;
+ case kBoolean:
+ // reset to current setting
+ event.getEntry().setBoolean(property.get() != 0);
+ return;
+ case kInteger:
+ case kEnum:
+ // reset to current setting
+ event.getEntry().setDouble(property.get());
+ return;
+ case kString:
+ // reset to current setting
+ event.getEntry().setString(property.getString());
+ return;
+ default:
+ return;
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * <p>You should call this method to see a camera feed on the dashboard.
+ * If you also want to perform vision processing on the roboRIO, use
+ * getVideo() to get access to the camera images.
+ *
+ * <p>The first time this overload is called, it calls
+ * {@link #startAutomaticCapture(int)} with device 0, creating a camera
+ * named "USB Camera 0". Subsequent calls increment the device number
+ * (e.g. 1, 2, etc).
+ */
+ public UsbCamera startAutomaticCapture() {
+ UsbCamera camera = startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * <p>This overload calls {@link #startAutomaticCapture(String, int)} with
+ * a name of "USB Camera {dev}".
+ *
+ * @param dev The device number of the camera interface
+ */
+ public UsbCamera startAutomaticCapture(int dev) {
+ UsbCamera camera = new UsbCamera("USB Camera " + dev, dev);
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * @param name The name to give the camera
+ * @param dev The device number of the camera interface
+ */
+ public UsbCamera startAutomaticCapture(String name, int dev) {
+ UsbCamera camera = new UsbCamera(name, dev);
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard.
+ *
+ * @param name The name to give the camera
+ * @param path The device path (e.g. "/dev/video0") of the camera
+ */
+ public UsbCamera startAutomaticCapture(String name, String path) {
+ UsbCamera camera = new UsbCamera(name, path);
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Start automatically capturing images to send to the dashboard from
+ * an existing camera.
+ *
+ * @param camera Camera
+ */
+ public void startAutomaticCapture(VideoSource camera) {
+ addCamera(camera);
+ VideoSink server = addServer("serve_" + camera.getName());
+ server.setSource(camera);
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * <p>This overload calls {@link #addAxisCamera(String, String)} with
+ * name "Axis Camera".
+ *
+ * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+ */
+ public AxisCamera addAxisCamera(String host) {
+ return addAxisCamera("Axis Camera", host);
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * <p>This overload calls {@link #addAxisCamera(String, String[])} with
+ * name "Axis Camera".
+ *
+ * @param hosts Array of Camera host IPs/DNS names
+ */
+ public AxisCamera addAxisCamera(String[] hosts) {
+ return addAxisCamera("Axis Camera", hosts);
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * @param name The name to give the camera
+ * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+ */
+ public AxisCamera addAxisCamera(String name, String host) {
+ AxisCamera camera = new AxisCamera(name, host);
+ // Create a passthrough MJPEG server for USB access
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Adds an Axis IP camera.
+ *
+ * @param name The name to give the camera
+ * @param hosts Array of Camera host IPs/DNS names
+ */
+ public AxisCamera addAxisCamera(String name, String[] hosts) {
+ AxisCamera camera = new AxisCamera(name, hosts);
+ // Create a passthrough MJPEG server for USB access
+ startAutomaticCapture(camera);
+ CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
+ return camera;
+ }
+
+ /**
+ * Get OpenCV access to the primary camera feed. This allows you to
+ * get images from the camera for image processing on the roboRIO.
+ *
+ * <p>This is only valid to call after a camera feed has been added
+ * with startAutomaticCapture() or addServer().
+ */
+ public CvSink getVideo() {
+ VideoSource source;
+ synchronized (this) {
+ if (m_primarySourceName == null) {
+ throw new VideoException("no camera available");
+ }
+ source = m_sources.get(m_primarySourceName);
+ }
+ if (source == null) {
+ throw new VideoException("no camera available");
+ }
+ return getVideo(source);
+ }
+
+ /**
+ * Get OpenCV access to the specified camera. This allows you to get
+ * images from the camera for image processing on the roboRIO.
+ *
+ * @param camera Camera (e.g. as returned by startAutomaticCapture).
+ */
+ public CvSink getVideo(VideoSource camera) {
+ String name = "opencv_" + camera.getName();
+
+ synchronized (this) {
+ VideoSink sink = m_sinks.get(name);
+ if (sink != null) {
+ VideoSink.Kind kind = sink.getKind();
+ if (kind != VideoSink.Kind.kCv) {
+ throw new VideoException("expected OpenCV sink, but got " + kind);
+ }
+ return (CvSink) sink;
+ }
+ }
+
+ CvSink newsink = new CvSink(name);
+ newsink.setSource(camera);
+ addServer(newsink);
+ return newsink;
+ }
+
+ /**
+ * Get OpenCV access to the specified camera. This allows you to get
+ * images from the camera for image processing on the roboRIO.
+ *
+ * @param name Camera name
+ */
+ public CvSink getVideo(String name) {
+ VideoSource source;
+ synchronized (this) {
+ source = m_sources.get(name);
+ if (source == null) {
+ throw new VideoException("could not find camera " + name);
+ }
+ }
+ return getVideo(source);
+ }
+
+ /**
+ * Create a MJPEG stream with OpenCV input. This can be called to pass custom
+ * annotated images to the dashboard.
+ *
+ * @param name Name to give the stream
+ * @param width Width of the image being sent
+ * @param height Height of the image being sent
+ */
+ public CvSource putVideo(String name, int width, int height) {
+ CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
+ startAutomaticCapture(source);
+ return source;
+ }
+
+ /**
+ * Adds a MJPEG server at the next available port.
+ *
+ * @param name Server name
+ */
+ public MjpegServer addServer(String name) {
+ int port;
+ synchronized (this) {
+ port = m_nextPort;
+ m_nextPort++;
+ }
+ return addServer(name, port);
+ }
+
+ /**
+ * Adds a MJPEG server.
+ *
+ * @param name Server name
+ */
+ public MjpegServer addServer(String name, int port) {
+ MjpegServer server = new MjpegServer(name, port);
+ addServer(server);
+ return server;
+ }
+
+ /**
+ * Adds an already created server.
+ *
+ * @param server Server
+ */
+ public void addServer(VideoSink server) {
+ synchronized (this) {
+ m_sinks.put(server.getName(), server);
+ }
+ }
+
+ /**
+ * Removes a server by name.
+ *
+ * @param name Server name
+ */
+ public void removeServer(String name) {
+ synchronized (this) {
+ m_sinks.remove(name);
+ }
+ }
+
+ /**
+ * Get server for the primary camera feed.
+ *
+ * <p>This is only valid to call after a camera feed has been added
+ * with startAutomaticCapture() or addServer().
+ */
+ public VideoSink getServer() {
+ synchronized (this) {
+ if (m_primarySourceName == null) {
+ throw new VideoException("no camera available");
+ }
+ return getServer("serve_" + m_primarySourceName);
+ }
+ }
+
+ /**
+ * Gets a server by name.
+ *
+ * @param name Server name
+ */
+ public VideoSink getServer(String name) {
+ synchronized (this) {
+ return m_sinks.get(name);
+ }
+ }
+
+ /**
+ * Adds an already created camera.
+ *
+ * @param camera Camera
+ */
+ public void addCamera(VideoSource camera) {
+ String name = camera.getName();
+ synchronized (this) {
+ if (m_primarySourceName == null) {
+ m_primarySourceName = name;
+ }
+ m_sources.put(name, camera);
+ }
+ }
+
+ /**
+ * Removes a camera by name.
+ *
+ * @param name Camera name
+ */
+ public void removeCamera(String name) {
+ synchronized (this) {
+ m_sources.remove(name);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CircularBuffer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CircularBuffer.java
new file mode 100644
index 0000000..7e8ecc4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CircularBuffer.java
@@ -0,0 +1,186 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
+ */
+public class CircularBuffer {
+ private double[] m_data;
+
+ // Index of element at front of buffer
+ private int m_front;
+
+ // Number of elements used in buffer
+ private int m_length;
+
+ /**
+ * Create a CircularBuffer with the provided size.
+ *
+ * @param size The size of the circular buffer.
+ */
+ public CircularBuffer(int size) {
+ m_data = new double[size];
+ for (int i = 0; i < m_data.length; i++) {
+ m_data[i] = 0.0;
+ }
+ }
+
+ /**
+ * Returns number of elements in buffer.
+ *
+ * @return number of elements in buffer
+ */
+ double size() {
+ return m_length;
+ }
+
+ /**
+ * Get value at front of buffer.
+ *
+ * @return value at front of buffer
+ */
+ double getFirst() {
+ return m_data[m_front];
+ }
+
+ /**
+ * Get value at back of buffer.
+ *
+ * @return value at back of buffer
+ */
+ double getLast() {
+ // If there are no elements in the buffer, do nothing
+ if (m_length == 0) {
+ return 0.0;
+ }
+
+ return m_data[(m_front + m_length - 1) % m_data.length];
+ }
+
+ /**
+ * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
+ * full.
+ */
+ public void addFirst(double value) {
+ if (m_data.length == 0) {
+ return;
+ }
+
+ m_front = moduloDec(m_front);
+
+ m_data[m_front] = value;
+
+ if (m_length < m_data.length) {
+ m_length++;
+ }
+ }
+
+ /**
+ * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
+ * full.
+ */
+ public void addLast(double value) {
+ if (m_data.length == 0) {
+ return;
+ }
+
+ m_data[(m_front + m_length) % m_data.length] = value;
+
+ if (m_length < m_data.length) {
+ m_length++;
+ } else {
+ // Increment front if buffer is full to maintain size
+ m_front = moduloInc(m_front);
+ }
+ }
+
+ /**
+ * Pop value at front of buffer.
+ *
+ * @return value at front of buffer
+ */
+ public double removeFirst() {
+ // If there are no elements in the buffer, do nothing
+ if (m_length == 0) {
+ return 0.0;
+ }
+
+ double temp = m_data[m_front];
+ m_front = moduloInc(m_front);
+ m_length--;
+ return temp;
+ }
+
+
+ /**
+ * Pop value at back of buffer.
+ */
+ public double removeLast() {
+ // If there are no elements in the buffer, do nothing
+ if (m_length == 0) {
+ return 0.0;
+ }
+
+ m_length--;
+ return m_data[(m_front + m_length) % m_data.length];
+ }
+
+ /**
+ * Resizes internal buffer to given size.
+ *
+ * <p>A new buffer is allocated because arrays are not resizable.
+ */
+ void resize(int size) {
+ double[] newBuffer = new double[size];
+ m_length = Math.min(m_length, size);
+ for (int i = 0; i < m_length; i++) {
+ newBuffer[i] = m_data[(m_front + i) % m_data.length];
+ }
+ m_data = newBuffer;
+ m_front = 0;
+ }
+
+ /**
+ * Sets internal buffer contents to zero.
+ */
+ public void clear() {
+ for (int i = 0; i < m_data.length; i++) {
+ m_data[i] = 0.0;
+ }
+ m_front = 0;
+ m_length = 0;
+ }
+
+ /**
+ * Get the element at the provided index relative to the start of the buffer.
+ *
+ * @return Element at index starting from front of buffer.
+ */
+ public double get(int index) {
+ return m_data[(m_front + index) % m_data.length];
+ }
+
+ /**
+ * Increment an index modulo the length of the m_data buffer.
+ */
+ private int moduloInc(int index) {
+ return (index + 1) % m_data.length;
+ }
+
+ /**
+ * Decrement an index modulo the length of the m_data buffer.
+ */
+ private int moduloDec(int index) {
+ if (index == 0) {
+ return m_data.length - 1;
+ } else {
+ return index - 1;
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
new file mode 100644
index 0000000..e95e8a7
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -0,0 +1,205 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.CompressorJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class for operating a compressor connected to a PCM (Pneumatic Control Module). The PCM will
+ * automatically run in closed loop mode by default whenever a {@link Solenoid} object is created.
+ * For most cases, a Compressor object does not need to be instantiated or used in a robot program.
+ * This class is only required in cases where the robot program needs a more detailed status of the
+ * compressor or to enable/disable closed loop control.
+ *
+ * <p>Note: you cannot operate the compressor directly from this class as doing so would circumvent
+ * the safety provided by using the pressure switch and closed loop control. You can only turn off
+ * closed loop control, thereby stopping the compressor from operating.
+ */
+public class Compressor extends SendableBase {
+ private int m_compressorHandle;
+ private byte m_module;
+
+ /**
+ * Makes a new instance of the compressor using the provided CAN device ID. Use this constructor
+ * when you have more than one PCM.
+ *
+ * @param module The PCM CAN device ID (0 - 62 inclusive)
+ */
+ public Compressor(int module) {
+ m_module = (byte) module;
+
+ m_compressorHandle = CompressorJNI.initializeCompressor((byte) module);
+
+ HAL.report(tResourceType.kResourceType_Compressor, module);
+ setName("Compressor", module);
+ }
+
+ /**
+ * Makes a new instance of the compressor using the default PCM ID of 0.
+ *
+ * <p>Additional modules can be supported by making a new instance and {@link #Compressor(int)
+ * specifying the CAN ID.}
+ */
+ public Compressor() {
+ this(SensorUtil.getDefaultSolenoidModule());
+ }
+
+ /**
+ * Start the compressor running in closed loop control mode.
+ *
+ * <p>Use the method in cases where you would like to manually stop and start the compressor for
+ * applications such as conserving battery or making sure that the compressor motor doesn't start
+ * during critical operations.
+ */
+ public void start() {
+ setClosedLoopControl(true);
+ }
+
+ /**
+ * Stop the compressor from running in closed loop control mode.
+ *
+ * <p>Use the method in cases where you would like to manually stop and start the compressor for
+ * applications such as conserving battery or making sure that the compressor motor doesn't start
+ * during critical operations.
+ */
+ public void stop() {
+ setClosedLoopControl(false);
+ }
+
+ /**
+ * Get the status of the compressor.
+ *
+ * @return true if the compressor is on
+ */
+ public boolean enabled() {
+ return CompressorJNI.getCompressor(m_compressorHandle);
+ }
+
+ /**
+ * Get the pressure switch value.
+ *
+ * @return true if the pressure is low
+ */
+ public boolean getPressureSwitchValue() {
+ return CompressorJNI.getCompressorPressureSwitch(m_compressorHandle);
+ }
+
+ /**
+ * Get the current being used by the compressor.
+ *
+ * @return current consumed by the compressor in amps
+ */
+ public double getCompressorCurrent() {
+ return CompressorJNI.getCompressorCurrent(m_compressorHandle);
+ }
+
+ /**
+ * Set the PCM in closed loop control mode.
+ *
+ * @param on if true sets the compressor to be in closed loop control mode (default)
+ */
+ public void setClosedLoopControl(boolean on) {
+ CompressorJNI.setCompressorClosedLoopControl(m_compressorHandle, on);
+ }
+
+ /**
+ * Gets the current operating mode of the PCM.
+ *
+ * @return true if compressor is operating on closed-loop mode
+ */
+ public boolean getClosedLoopControl() {
+ return CompressorJNI.getCompressorClosedLoopControl(m_compressorHandle);
+ }
+
+ /**
+ * If PCM is in fault state : Compressor Drive is disabled due to compressor current being too
+ * high.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getCompressorCurrentTooHighFault() {
+ return CompressorJNI.getCompressorCurrentTooHighFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM sticky fault is set : Compressor is disabled due to compressor current being too
+ * high.
+ *
+ * @return true if PCM sticky fault is set.
+ */
+ public boolean getCompressorCurrentTooHighStickyFault() {
+ return CompressorJNI.getCompressorCurrentTooHighStickyFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM sticky fault is set : Compressor output appears to be shorted.
+ *
+ * @return true if PCM sticky fault is set.
+ */
+ public boolean getCompressorShortedStickyFault() {
+ return CompressorJNI.getCompressorShortedStickyFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM is in fault state : Compressor output appears to be shorted.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getCompressorShortedFault() {
+ return CompressorJNI.getCompressorShortedFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM sticky fault is set : Compressor does not appear to be wired, i.e. compressor is not
+ * drawing enough current.
+ *
+ * @return true if PCM sticky fault is set.
+ */
+ public boolean getCompressorNotConnectedStickyFault() {
+ return CompressorJNI.getCompressorNotConnectedStickyFault(m_compressorHandle);
+ }
+
+ /**
+ * If PCM is in fault state : Compressor does not appear to be wired, i.e. compressor is not
+ * drawing enough current.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getCompressorNotConnectedFault() {
+ return CompressorJNI.getCompressorNotConnectedFault(m_compressorHandle);
+ }
+
+ /**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * <p>If a sticky fault is set, then it will be persistently cleared. The compressor might
+ * momentarily disable while the flags are being cleared. Doo not call this method too
+ * frequently, otherwise normal compressor functionality may be prevented.
+ *
+ * <p>If no sticky faults are set then this call will have no effect.
+ */
+ public void clearAllPCMStickyFaults() {
+ CompressorJNI.clearAllPCMStickyFaults(m_module);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Compressor");
+ builder.addBooleanProperty("Enabled", this::enabled, value -> {
+ if (value) {
+ start();
+ } else {
+ stop();
+ }
+ });
+ builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
new file mode 100644
index 0000000..32fc7c3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * An interface for controllers. Controllers run control loops, the most command are PID controllers
+ * and there variants, but this includes anything that is controlling an actuator in a separate
+ * thread.
+ */
+public interface Controller {
+ /**
+ * Allows the control loop to run.
+ */
+ void enable();
+
+ /**
+ * Stops the control loop from running until explicitly re-enabled by calling {@link #enable()}.
+ */
+ void disable();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ControllerPower.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ControllerPower.java
new file mode 100644
index 0000000..3a03c60
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ControllerPower.java
@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.PowerJNI;
+
+/**
+ * Old Controller PR class.
+ * @deprecated Use RobotController class instead
+ */
+@Deprecated
+public final class ControllerPower {
+ /**
+ * Get the input voltage to the robot controller.
+ *
+ * @return The controller input voltage value in Volts
+ */
+ @Deprecated
+ public static double getInputVoltage() {
+ return PowerJNI.getVinVoltage();
+ }
+
+ /**
+ * Get the input current to the robot controller.
+ *
+ * @return The controller input current value in Amps
+ */
+ @Deprecated
+ public static double getInputCurrent() {
+ return PowerJNI.getVinCurrent();
+ }
+
+ /**
+ * Get the voltage of the 3.3V rail.
+ *
+ * @return The controller 3.3V rail voltage value in Volts
+ */
+ @Deprecated
+ public static double getVoltage3V3() {
+ return PowerJNI.getUserVoltage3V3();
+ }
+
+ /**
+ * Get the current output of the 3.3V rail.
+ *
+ * @return The controller 3.3V rail output current value in Volts
+ */
+ @Deprecated
+ public static double getCurrent3V3() {
+ return PowerJNI.getUserCurrent3V3();
+ }
+
+ /**
+ * Get the enabled state of the 3.3V rail. The rail may be disabled due to a controller brownout,
+ * a short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 3.3V rail enabled value
+ */
+ @Deprecated
+ public static boolean getEnabled3V3() {
+ return PowerJNI.getUserActive3V3();
+ }
+
+ /**
+ * Get the count of the total current faults on the 3.3V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ @Deprecated
+ public static int getFaultCount3V3() {
+ return PowerJNI.getUserCurrentFaults3V3();
+ }
+
+ /**
+ * Get the voltage of the 5V rail.
+ *
+ * @return The controller 5V rail voltage value in Volts
+ */
+ @Deprecated
+ public static double getVoltage5V() {
+ return PowerJNI.getUserVoltage5V();
+ }
+
+ /**
+ * Get the current output of the 5V rail.
+ *
+ * @return The controller 5V rail output current value in Amps
+ */
+ @Deprecated
+ public static double getCurrent5V() {
+ return PowerJNI.getUserCurrent5V();
+ }
+
+ /**
+ * Get the enabled state of the 5V rail. The rail may be disabled due to a controller brownout, a
+ * short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 5V rail enabled value
+ */
+ @Deprecated
+ public static boolean getEnabled5V() {
+ return PowerJNI.getUserActive5V();
+ }
+
+ /**
+ * Get the count of the total current faults on the 5V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ @Deprecated
+ public static int getFaultCount5V() {
+ return PowerJNI.getUserCurrentFaults5V();
+ }
+
+ /**
+ * Get the voltage of the 6V rail.
+ *
+ * @return The controller 6V rail voltage value in Volts
+ */
+ @Deprecated
+ public static double getVoltage6V() {
+ return PowerJNI.getUserVoltage6V();
+ }
+
+ /**
+ * Get the current output of the 6V rail.
+ *
+ * @return The controller 6V rail output current value in Amps
+ */
+ @Deprecated
+ public static double getCurrent6V() {
+ return PowerJNI.getUserCurrent6V();
+ }
+
+ /**
+ * Get the enabled state of the 6V rail. The rail may be disabled due to a controller brownout, a
+ * short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 6V rail enabled value
+ */
+ @Deprecated
+ public static boolean getEnabled6V() {
+ return PowerJNI.getUserActive6V();
+ }
+
+ /**
+ * Get the count of the total current faults on the 6V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ @Deprecated
+ public static int getFaultCount6V() {
+ return PowerJNI.getUserCurrentFaults6V();
+ }
+
+ private ControllerPower() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
new file mode 100644
index 0000000..dca332a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -0,0 +1,563 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+import edu.wpi.first.hal.CounterJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Class for counting the number of ticks on a digital input channel.
+ *
+ * <p>This is a general purpose class for counting repetitive events. It can return the number of
+ * counts, the period of the most recent cycle, and detect when the signal being counted has
+ * stopped by supplying a maximum cycle time.
+ *
+ * <p>All counters will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public class Counter extends SendableBase implements CounterBase, PIDSource {
+ /**
+ * Mode determines how and what the counter counts.
+ */
+ public enum Mode {
+ /**
+ * mode: two pulse.
+ */
+ kTwoPulse(0),
+ /**
+ * mode: semi period.
+ */
+ kSemiperiod(1),
+ /**
+ * mode: pulse length.
+ */
+ kPulseLength(2),
+ /**
+ * mode: external direction.
+ */
+ kExternalDirection(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Mode(int value) {
+ this.value = value;
+ }
+ }
+
+ protected DigitalSource m_upSource; // /< What makes the counter count up.
+ protected DigitalSource m_downSource; // /< What makes the counter count down.
+ private boolean m_allocatedUpSource;
+ private boolean m_allocatedDownSource;
+ private int m_counter; // /< The FPGA counter object.
+ private int m_index; // /< The index of this counter.
+ private PIDSourceType m_pidSource;
+ private double m_distancePerPulse; // distance of travel for each tick
+
+ /**
+ * Create an instance of a counter with the given mode.
+ */
+ public Counter(final Mode mode) {
+ ByteBuffer index = ByteBuffer.allocateDirect(4);
+ // set the byte order
+ index.order(ByteOrder.LITTLE_ENDIAN);
+ m_counter = CounterJNI.initializeCounter(mode.value, index.asIntBuffer());
+ m_index = index.asIntBuffer().get(0);
+
+ m_allocatedUpSource = false;
+ m_allocatedDownSource = false;
+ m_upSource = null;
+ m_downSource = null;
+
+ setMaxPeriod(.5);
+
+ HAL.report(tResourceType.kResourceType_Counter, m_index, mode.value);
+ setName("Counter", m_index);
+ }
+
+ /**
+ * Create an instance of a counter where no sources are selected. Then they all must be selected
+ * by calling functions to specify the upsource and the downsource independently.
+ *
+ * <p>The counter will start counting immediately.
+ */
+ public Counter() {
+ this(Mode.kTwoPulse);
+ }
+
+ /**
+ * Create an instance of a counter from a Digital Input. This is used if an existing digital input
+ * is to be shared by multiple other objects such as encoders or if the Digital Source is not a
+ * DIO channel (such as an Analog Trigger)
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param source the digital source to count
+ */
+ public Counter(DigitalSource source) {
+ this();
+
+ requireNonNull(source, "Digital Source given was null");
+ setUpSource(source);
+ }
+
+ /**
+ * Create an instance of a Counter object. Create an up-Counter instance given a channel.
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param channel the DIO channel to use as the up source. 0-9 are on-board, 10-25 are on the MXP
+ */
+ public Counter(int channel) {
+ this();
+ setUpSource(channel);
+ }
+
+ /**
+ * Create an instance of a Counter object. Create an instance of a simple up-Counter given an
+ * analog trigger. Use the trigger state output from the analog trigger.
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param encodingType which edges to count
+ * @param upSource first source to count
+ * @param downSource second source for direction
+ * @param inverted true to invert the count
+ */
+ public Counter(EncodingType encodingType, DigitalSource upSource, DigitalSource downSource,
+ boolean inverted) {
+ this(Mode.kExternalDirection);
+
+ requireNonNull(encodingType, "Encoding type given was null");
+ requireNonNull(upSource, "Up Source given was null");
+ requireNonNull(downSource, "Down Source given was null");
+
+ if (encodingType != EncodingType.k1X && encodingType != EncodingType.k2X) {
+ throw new IllegalArgumentException("Counters only support 1X and 2X quadrature decoding!");
+ }
+
+ setUpSource(upSource);
+ setDownSource(downSource);
+
+ if (encodingType == EncodingType.k1X) {
+ setUpSourceEdge(true, false);
+ CounterJNI.setCounterAverageSize(m_counter, 1);
+ } else {
+ setUpSourceEdge(true, true);
+ CounterJNI.setCounterAverageSize(m_counter, 2);
+ }
+
+ setDownSourceEdge(inverted, true);
+ }
+
+ /**
+ * Create an instance of a Counter object. Create an instance of a simple up-Counter given an
+ * analog trigger. Use the trigger state output from the analog trigger.
+ *
+ * <p>The counter will start counting immediately.
+ *
+ * @param trigger the analog trigger to count
+ */
+ public Counter(AnalogTrigger trigger) {
+ this();
+
+ requireNonNull(trigger, "The Analog Trigger given was null");
+
+ setUpSource(trigger.createOutput(AnalogTriggerType.kState));
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ setUpdateWhenEmpty(true);
+
+ clearUpSource();
+ clearDownSource();
+
+ CounterJNI.freeCounter(m_counter);
+
+ m_upSource = null;
+ m_downSource = null;
+ m_counter = 0;
+ }
+
+ /**
+ * The counter's FPGA index.
+ *
+ * @return the Counter's FPGA index
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public int getFPGAIndex() {
+ return m_index;
+ }
+
+ /**
+ * Set the upsource for the counter as a digital input channel.
+ *
+ * @param channel the DIO channel to count 0-9 are on-board, 10-25 are on the MXP
+ */
+ public void setUpSource(int channel) {
+ setUpSource(new DigitalInput(channel));
+ m_allocatedUpSource = true;
+ addChild(m_upSource);
+ }
+
+ /**
+ * Set the source object that causes the counter to count up. Set the up counting DigitalSource.
+ *
+ * @param source the digital source to count
+ */
+ public void setUpSource(DigitalSource source) {
+ if (m_upSource != null && m_allocatedUpSource) {
+ m_upSource.close();
+ m_allocatedUpSource = false;
+ }
+ m_upSource = source;
+ CounterJNI.setCounterUpSource(m_counter, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting());
+ }
+
+ /**
+ * Set the up counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Up Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+ public void setUpSource(AnalogTrigger analogTrigger, AnalogTriggerType triggerType) {
+ requireNonNull(analogTrigger, "Analog Trigger given was null");
+ requireNonNull(triggerType, "Analog Trigger Type given was null");
+
+ setUpSource(analogTrigger.createOutput(triggerType));
+ m_allocatedUpSource = true;
+ }
+
+ /**
+ * Set the edge sensitivity on an up counting source. Set the up source to either detect rising
+ * edges or falling edges.
+ *
+ * @param risingEdge true to count rising edge
+ * @param fallingEdge true to count falling edge
+ */
+ public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
+ if (m_upSource == null) {
+ throw new IllegalStateException("Up Source must be set before setting the edge!");
+ }
+ CounterJNI.setCounterUpSourceEdge(m_counter, risingEdge, fallingEdge);
+ }
+
+ /**
+ * Disable the up counting source to the counter.
+ */
+ public void clearUpSource() {
+ if (m_upSource != null && m_allocatedUpSource) {
+ m_upSource.close();
+ m_allocatedUpSource = false;
+ }
+ m_upSource = null;
+
+ CounterJNI.clearCounterUpSource(m_counter);
+ }
+
+ /**
+ * Set the down counting source to be a digital input channel.
+ *
+ * @param channel the DIO channel to count 0-9 are on-board, 10-25 are on the MXP
+ */
+ public void setDownSource(int channel) {
+ setDownSource(new DigitalInput(channel));
+ m_allocatedDownSource = true;
+ addChild(m_downSource);
+ }
+
+ /**
+ * Set the source object that causes the counter to count down. Set the down counting
+ * DigitalSource.
+ *
+ * @param source the digital source to count
+ */
+ public void setDownSource(DigitalSource source) {
+ requireNonNull(source, "The Digital Source given was null");
+
+ if (m_downSource != null && m_allocatedDownSource) {
+ m_downSource.close();
+ m_allocatedDownSource = false;
+ }
+ CounterJNI.setCounterDownSource(m_counter, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting());
+ m_downSource = source;
+ }
+
+ /**
+ * Set the down counting source to be an analog trigger.
+ *
+ * @param analogTrigger The analog trigger object that is used for the Down Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+ public void setDownSource(AnalogTrigger analogTrigger, AnalogTriggerType triggerType) {
+ requireNonNull(analogTrigger, "Analog Trigger given was null");
+ requireNonNull(triggerType, "Analog Trigger Type given was null");
+
+ setDownSource(analogTrigger.createOutput(triggerType));
+ m_allocatedDownSource = true;
+ }
+
+ /**
+ * Set the edge sensitivity on a down counting source. Set the down source to either detect rising
+ * edges or falling edges.
+ *
+ * @param risingEdge true to count the rising edge
+ * @param fallingEdge true to count the falling edge
+ */
+ public void setDownSourceEdge(boolean risingEdge, boolean fallingEdge) {
+ requireNonNull(m_downSource, "Down Source must be set before setting the edge!");
+
+ CounterJNI.setCounterDownSourceEdge(m_counter, risingEdge, fallingEdge);
+ }
+
+ /**
+ * Disable the down counting source to the counter.
+ */
+ public void clearDownSource() {
+ if (m_downSource != null && m_allocatedDownSource) {
+ m_downSource.close();
+ m_allocatedDownSource = false;
+ }
+ m_downSource = null;
+
+ CounterJNI.clearCounterDownSource(m_counter);
+ }
+
+ /**
+ * Set standard up / down counting mode on this counter. Up and down counts are sourced
+ * independently from two inputs.
+ */
+ public void setUpDownCounterMode() {
+ CounterJNI.setCounterUpDownMode(m_counter);
+ }
+
+ /**
+ * Set external direction mode on this counter. Counts are sourced on the Up counter input. The
+ * Down counter input represents the direction to count.
+ */
+ public void setExternalDirectionMode() {
+ CounterJNI.setCounterExternalDirectionMode(m_counter);
+ }
+
+ /**
+ * Set Semi-period mode on this counter. Counts up on both rising and falling edges.
+ *
+ * @param highSemiPeriod true to count up on both rising and falling
+ */
+ public void setSemiPeriodMode(boolean highSemiPeriod) {
+ CounterJNI.setCounterSemiPeriodMode(m_counter, highSemiPeriod);
+ }
+
+ /**
+ * Configure the counter to count in up or down based on the length of the input pulse. This mode
+ * is most useful for direction sensitive gear tooth sensors.
+ *
+ * @param threshold The pulse length beyond which the counter counts the opposite direction. Units
+ * are seconds.
+ */
+ public void setPulseLengthMode(double threshold) {
+ CounterJNI.setCounterPulseLengthMode(m_counter, threshold);
+ }
+
+ /**
+ * Read the current counter value. Read the value at this instant. It may still be running, so it
+ * reflects the current value. Next time it is read, it might have a different value.
+ */
+ @Override
+ public int get() {
+ return CounterJNI.getCounter(m_counter);
+ }
+
+ /**
+ * Read the current scaled counter value. Read the value at this instant, scaled by the distance
+ * per pulse (defaults to 1).
+ *
+ * @return The distance since the last reset
+ */
+ public double getDistance() {
+ return get() * m_distancePerPulse;
+ }
+
+ /**
+ * Reset the Counter to zero. Set the counter value to zero. This doesn't effect the running state
+ * of the counter, just sets the current value to zero.
+ */
+ @Override
+ public void reset() {
+ CounterJNI.resetCounter(m_counter);
+ }
+
+ /**
+ * Set the maximum period where the device is still considered "moving". Sets the maximum period
+ * where the device is considered moving. This value is used to determine the "stopped" state of
+ * the counter using the GetStopped method.
+ *
+ * @param maxPeriod The maximum period where the counted device is considered moving in seconds.
+ */
+ @Override
+ public void setMaxPeriod(double maxPeriod) {
+ CounterJNI.setCounterMaxPeriod(m_counter, maxPeriod);
+ }
+
+ /**
+ * Select whether you want to continue updating the event timer output when there are no samples
+ * captured. The output of the event timer has a buffer of periods that are averaged and posted to
+ * a register on the FPGA. When the timer detects that the event source has stopped (based on the
+ * MaxPeriod) the buffer of samples to be averaged is emptied. If you enable the update when
+ * empty, you will be notified of the stopped source and the event time will report 0 samples. If
+ * you disable update when empty, the most recent average will remain on the output until a new
+ * sample is acquired. You will never see 0 samples output (except when there have been no events
+ * since an FPGA reset) and you will likely not see the stopped bit become true (since it is
+ * updated at the end of an average and there are no samples to average).
+ *
+ * @param enabled true to continue updating
+ */
+ public void setUpdateWhenEmpty(boolean enabled) {
+ CounterJNI.setCounterUpdateWhenEmpty(m_counter, enabled);
+ }
+
+ /**
+ * Determine if the clock is stopped. Determine if the clocked input is stopped based on the
+ * MaxPeriod value set using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
+ * device (and counter) are assumed to be stopped and it returns true.
+ *
+ * @return true if the most recent counter period exceeds the MaxPeriod value set by SetMaxPeriod.
+ */
+ @Override
+ public boolean getStopped() {
+ return CounterJNI.getCounterStopped(m_counter);
+ }
+
+ /**
+ * The last direction the counter value changed.
+ *
+ * @return The last direction the counter value changed.
+ */
+ @Override
+ public boolean getDirection() {
+ return CounterJNI.getCounterDirection(m_counter);
+ }
+
+ /**
+ * Set the Counter to return reversed sensing on the direction. This allows counters to change the
+ * direction they are counting in the case of 1X and 2X quadrature encoding only. Any other
+ * counter mode isn't supported.
+ *
+ * @param reverseDirection true if the value counted should be negated.
+ */
+ public void setReverseDirection(boolean reverseDirection) {
+ CounterJNI.setCounterReverseDirection(m_counter, reverseDirection);
+ }
+
+ /**
+ * Get the Period of the most recent count. Returns the time interval of the most recent count.
+ * This can be used for velocity calculations to determine shaft speed.
+ *
+ * @return The period of the last two pulses in units of seconds.
+ */
+ @Override
+ public double getPeriod() {
+ return CounterJNI.getCounterPeriod(m_counter);
+ }
+
+ /**
+ * Get the current rate of the Counter. Read the current rate of the counter accounting for the
+ * distance per pulse value. The default value for distance per pulse (1) yields units of pulses
+ * per second.
+ *
+ * @return The rate in units/sec
+ */
+ public double getRate() {
+ return m_distancePerPulse / getPeriod();
+ }
+
+ /**
+ * Set the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+ public void setSamplesToAverage(int samplesToAverage) {
+ CounterJNI.setCounterSamplesToAverage(m_counter, samplesToAverage);
+ }
+
+ /**
+ * Get the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+ public int getSamplesToAverage() {
+ return CounterJNI.getCounterSamplesToAverage(m_counter);
+ }
+
+ /**
+ * Set the distance per pulse for this counter. This sets the multiplier used to determine the
+ * distance driven based on the count value from the encoder. Set this value based on the Pulses
+ * per Revolution and factor in any gearing reductions. This distance can be in any units you
+ * like, linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+ */
+ public void setDistancePerPulse(double distancePerPulse) {
+ m_distancePerPulse = distancePerPulse;
+ }
+
+ /**
+ * Set which parameter of the encoder you are using as a process control variable. The counter
+ * class supports the rate and distance parameters.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ requireNonNull(pidSource, "PID Source Parameter given was null");
+ if (pidSource != PIDSourceType.kDisplacement && pidSource != PIDSourceType.kRate) {
+ throw new IllegalArgumentException("PID Source parameter was not valid type: " + pidSource);
+ }
+
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ @Override
+ public double pidGet() {
+ switch (m_pidSource) {
+ case kDisplacement:
+ return getDistance();
+ case kRate:
+ return getRate();
+ default:
+ return 0.0;
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Counter");
+ builder.addDoubleProperty("Value", this::get, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
new file mode 100644
index 0000000..9d1d6b7
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for counting the number of ticks on a digital input channel. Encoders, Gear tooth
+ * sensors, and counters should all subclass this so it can be used to build more advanced classes
+ * for control and driving.
+ *
+ * <p>All counters will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public interface CounterBase {
+ /**
+ * The number of edges for the counterbase to increment or decrement on.
+ */
+ enum EncodingType {
+ /**
+ * Count only the rising edge.
+ */
+ k1X(0),
+ /**
+ * Count both the rising and falling edge.
+ */
+ k2X(1),
+ /**
+ * Count rising and falling on both channels.
+ */
+ k4X(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ EncodingType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Get the count.
+ *
+ * @return the count
+ */
+ int get();
+
+ /**
+ * Reset the count to zero.
+ */
+ void reset();
+
+ /**
+ * Get the time between the last two edges counted.
+ *
+ * @return the time between the last two ticks in seconds
+ */
+ double getPeriod();
+
+ /**
+ * Set the maximum time between edges to be considered stalled.
+ *
+ * @param maxPeriod the maximum period in seconds
+ */
+ void setMaxPeriod(double maxPeriod);
+
+ /**
+ * Determine if the counter is not moving.
+ *
+ * @return true if the counter has not changed for the max period
+ */
+ boolean getStopped();
+
+ /**
+ * Determine which direction the counter is going.
+ *
+ * @return true for one direction, false for the other
+ */
+ boolean getDirection();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
new file mode 100644
index 0000000..c2f4c9c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Digilent DMC 60 Speed Controller.
+ */
+public class DMC60 extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * <p>Note that the DMC uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the DMC 60 User Manual
+ * available from Digilent
+ *
+ * <p>- 2.004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
+ * center of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms =
+ * full "reverse"
+ *
+ * @param channel The PWM channel that the DMC60 is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ public DMC60(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, .997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel());
+ setName("DMC60", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
new file mode 100644
index 0000000..ad92f48
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
@@ -0,0 +1,181 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.DigitalGlitchFilterJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class to enable glitch filtering on a set of digital inputs. This class will manage adding and
+ * removing digital inputs from a FPGA glitch filter. The filter lets the user configure the time
+ * that an input must remain high or low before it is classified as high or low.
+ */
+public class DigitalGlitchFilter extends SendableBase {
+ /**
+ * Configures the Digital Glitch Filter to its default settings.
+ */
+ public DigitalGlitchFilter() {
+ synchronized (m_mutex) {
+ int index = 0;
+ while (m_filterAllocated[index] && index < m_filterAllocated.length) {
+ index++;
+ }
+ if (index != m_filterAllocated.length) {
+ m_channelIndex = index;
+ m_filterAllocated[index] = true;
+ HAL.report(tResourceType.kResourceType_DigitalGlitchFilter,
+ m_channelIndex, 0);
+ setName("DigitalGlitchFilter", index);
+ }
+ }
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ if (m_channelIndex >= 0) {
+ synchronized (m_mutex) {
+ m_filterAllocated[m_channelIndex] = false;
+ }
+ m_channelIndex = -1;
+ }
+ }
+
+ private static void setFilter(DigitalSource input, int channelIndex) {
+ if (input != null) { // Counter might have just one input
+ // analog triggers are not supported for DigitalGlitchFilters
+ if (input.isAnalogTrigger()) {
+ throw new IllegalStateException("Analog Triggers not supported for DigitalGlitchFilters");
+ }
+ DigitalGlitchFilterJNI.setFilterSelect(input.getPortHandleForRouting(), channelIndex);
+
+ int selected = DigitalGlitchFilterJNI.getFilterSelect(input.getPortHandleForRouting());
+ if (selected != channelIndex) {
+ throw new IllegalStateException("DigitalGlitchFilterJNI.setFilterSelect("
+ + channelIndex + ") failed -> " + selected);
+ }
+ }
+ }
+
+ /**
+ * Assigns the DigitalSource to this glitch filter.
+ *
+ * @param input The DigitalSource to add.
+ */
+ public void add(DigitalSource input) {
+ setFilter(input, m_channelIndex + 1);
+ }
+
+ /**
+ * Assigns the Encoder to this glitch filter.
+ *
+ * @param input The Encoder to add.
+ */
+ public void add(Encoder input) {
+ add(input.m_aSource);
+ add(input.m_bSource);
+ }
+
+ /**
+ * Assigns the Counter to this glitch filter.
+ *
+ * @param input The Counter to add.
+ */
+ public void add(Counter input) {
+ add(input.m_upSource);
+ add(input.m_downSource);
+ }
+
+ /**
+ * Removes this filter from the given digital input.
+ *
+ * @param input The DigitalSource to stop filtering.
+ */
+ public void remove(DigitalSource input) {
+ setFilter(input, 0);
+ }
+
+ /**
+ * Removes this filter from the given Encoder.
+ *
+ * @param input the Encoder to stop filtering.
+ */
+ public void remove(Encoder input) {
+ remove(input.m_aSource);
+ remove(input.m_bSource);
+ }
+
+ /**
+ * Removes this filter from the given Counter.
+ *
+ * @param input The Counter to stop filtering.
+ */
+ public void remove(Counter input) {
+ remove(input.m_upSource);
+ remove(input.m_downSource);
+ }
+
+ /**
+ * Sets the number of FPGA cycles that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @param fpgaCycles The number of FPGA cycles.
+ */
+ public void setPeriodCycles(int fpgaCycles) {
+ DigitalGlitchFilterJNI.setFilterPeriod(m_channelIndex, fpgaCycles);
+ }
+
+ /**
+ * Sets the number of nanoseconds that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @param nanoseconds The number of nanoseconds.
+ */
+ public void setPeriodNanoSeconds(long nanoseconds) {
+ int fpgaCycles = (int) (nanoseconds * SensorUtil.kSystemClockTicksPerMicrosecond / 4
+ / 1000);
+ setPeriodCycles(fpgaCycles);
+ }
+
+ /**
+ * Gets the number of FPGA cycles that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @return The number of cycles.
+ */
+ public int getPeriodCycles() {
+ return DigitalGlitchFilterJNI.getFilterPeriod(m_channelIndex);
+ }
+
+ /**
+ * Gets the number of nanoseconds that the input must hold steady to pass through this glitch
+ * filter.
+ *
+ * @return The number of nanoseconds.
+ */
+ public long getPeriodNanoSeconds() {
+ int fpgaCycles = getPeriodCycles();
+
+ return (long) fpgaCycles * 1000L
+ / (long) (SensorUtil.kSystemClockTicksPerMicrosecond / 4);
+ }
+
+ @Override
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ public void initSendable(SendableBuilder builder) {
+ }
+
+ private int m_channelIndex = -1;
+ private static final Lock m_mutex = new ReentrantLock(true);
+ private static final boolean[] m_filterAllocated = new boolean[3];
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
new file mode 100644
index 0000000..03a0f21
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class to read a digital input. This class will read digital inputs and return the current value
+ * on the channel. Other devices such as encoders, gear tooth sensors, etc. that are implemented
+ * elsewhere will automatically allocate digital inputs and outputs as required. This class is only
+ * for devices like switches etc. that aren't implemented anywhere else.
+ */
+public class DigitalInput extends DigitalSource {
+ private final int m_channel;
+ private int m_handle;
+
+ /**
+ * Create an instance of a Digital Input class. Creates a digital input given a channel.
+ *
+ * @param channel the DIO channel for the digital input 0-9 are on-board, 10-25 are on the MXP
+ */
+ public DigitalInput(int channel) {
+ SensorUtil.checkDigitalChannel(channel);
+ m_channel = channel;
+
+ m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), true);
+
+ HAL.report(tResourceType.kResourceType_DigitalInput, channel);
+ setName("DigitalInput", channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ if (m_interrupt != 0) {
+ cancelInterrupts();
+ }
+ DIOJNI.freeDIOPort(m_handle);
+ m_handle = 0;
+ }
+
+ /**
+ * Get the value from a digital input channel. Retrieve the value of a single digital input
+ * channel from the FPGA.
+ *
+ * @return the status of the digital input
+ */
+ public boolean get() {
+ return DIOJNI.getDIO(m_handle);
+ }
+
+ /**
+ * Get the channel of the digital input.
+ *
+ * @return The GPIO channel number that this object represents.
+ */
+ @Override
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Get the analog trigger type.
+ *
+ * @return false
+ */
+ @Override
+ public int getAnalogTriggerTypeForRouting() {
+ return 0;
+ }
+
+ /**
+ * Is this an analog trigger.
+ *
+ * @return true if this is an analog trigger
+ */
+ @Override
+ public boolean isAnalogTrigger() {
+ return false;
+ }
+
+ /**
+ * Get the HAL Port Handle.
+ *
+ * @return The HAL Handle to the specified source.
+ */
+ @Override
+ public int getPortHandleForRouting() {
+ return m_handle;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Digital Input");
+ builder.addBooleanProperty("Value", this::get, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
new file mode 100644
index 0000000..f375ccb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class to write digital outputs. This class will write digital outputs. Other devices that are
+ * implemented elsewhere will automatically allocate digital inputs and outputs as required.
+ */
+public class DigitalOutput extends SendableBase {
+ private static final int invalidPwmGenerator = 0;
+ private int m_pwmGenerator = invalidPwmGenerator;
+
+ private final int m_channel;
+ private int m_handle;
+
+ /**
+ * Create an instance of a digital output. Create an instance of a digital output given a
+ * channel.
+ *
+ * @param channel the DIO channel to use for the digital output. 0-9 are on-board, 10-25 are on
+ * the MXP
+ */
+ public DigitalOutput(int channel) {
+ SensorUtil.checkDigitalChannel(channel);
+ m_channel = channel;
+
+ m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), false);
+
+ HAL.report(tResourceType.kResourceType_DigitalOutput, channel);
+ setName("DigitalOutput", channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ // Disable the pwm only if we have allocated it
+ if (m_pwmGenerator != invalidPwmGenerator) {
+ disablePWM();
+ }
+ DIOJNI.freeDIOPort(m_handle);
+ m_handle = 0;
+ }
+
+ /**
+ * Set the value of a digital output.
+ *
+ * @param value true is on, off is false
+ */
+ public void set(boolean value) {
+ DIOJNI.setDIO(m_handle, (short) (value ? 1 : 0));
+ }
+
+ /**
+ * Gets the value being output from the Digital Output.
+ *
+ * @return the state of the digital output.
+ */
+ public boolean get() {
+ return DIOJNI.getDIO(m_handle);
+ }
+
+ /**
+ * Get the GPIO channel number that this object represents.
+ *
+ * @return The GPIO channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Generate a single pulse. There can only be a single pulse going at any time.
+ *
+ * @param pulseLength The length of the pulse.
+ */
+ public void pulse(final double pulseLength) {
+ DIOJNI.pulse(m_handle, pulseLength);
+ }
+
+ /**
+ * Determine if the pulse is still going. Determine if a previously started pulse is still going.
+ *
+ * @return true if pulsing
+ */
+ public boolean isPulsing() {
+ return DIOJNI.isPulsing(m_handle);
+ }
+
+ /**
+ * Change the PWM frequency of the PWM output on a Digital Output line.
+ *
+ * <p>The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic.
+ *
+ * <p>There is only one PWM frequency for all channels.
+ *
+ * @param rate The frequency to output all digital output PWM signals.
+ */
+ public void setPWMRate(double rate) {
+ DIOJNI.setDigitalPWMRate(rate);
+ }
+
+ /**
+ * Enable a PWM Output on this line.
+ *
+ * <p>Allocate one of the 6 DO PWM generator resources.
+ *
+ * <p>Supply the initial duty-cycle to output so as to avoid a glitch when first starting.
+ *
+ * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
+ * the higher the frequency of the PWM signal is.
+ *
+ * @param initialDutyCycle The duty-cycle to start generating. [0..1]
+ */
+ public void enablePWM(double initialDutyCycle) {
+ if (m_pwmGenerator != invalidPwmGenerator) {
+ return;
+ }
+ m_pwmGenerator = DIOJNI.allocateDigitalPWM();
+ DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle);
+ DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, m_channel);
+ }
+
+ /**
+ * Change this line from a PWM output back to a static Digital Output line.
+ *
+ * <p>Free up one of the 6 DO PWM generator resources that were in use.
+ */
+ public void disablePWM() {
+ if (m_pwmGenerator == invalidPwmGenerator) {
+ return;
+ }
+ // Disable the output by routing to a dead bit.
+ DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil.kDigitalChannels);
+ DIOJNI.freeDigitalPWM(m_pwmGenerator);
+ m_pwmGenerator = invalidPwmGenerator;
+ }
+
+ /**
+ * Change the duty-cycle that is being generated on the line.
+ *
+ * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
+ * the
+ * higher the frequency of the PWM signal is.
+ *
+ * @param dutyCycle The duty-cycle to change to. [0..1]
+ */
+ public void updateDutyCycle(double dutyCycle) {
+ if (m_pwmGenerator == invalidPwmGenerator) {
+ return;
+ }
+ DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Digital Output");
+ builder.addBooleanProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
new file mode 100644
index 0000000..d31e191
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * DigitalSource Interface. The DigitalSource represents all the possible inputs for a counter or a
+ * quadrature encoder. The source may be either a digital input or an analog input. If the caller
+ * just provides a channel, then a digital input will be constructed and freed when finished for the
+ * source. The source can either be a digital input or analog trigger but not both.
+ */
+public abstract class DigitalSource extends InterruptableSensorBase {
+ public abstract boolean isAnalogTrigger();
+
+ public abstract int getChannel();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
new file mode 100644
index 0000000..8cd60ec
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -0,0 +1,181 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SolenoidJNI;
+import edu.wpi.first.hal.util.UncleanStatusException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the PCM.
+ *
+ * <p>The DoubleSolenoid class is typically used for pneumatics solenoids that have two positions
+ * controlled by two separate channels.
+ */
+public class DoubleSolenoid extends SolenoidBase {
+ /**
+ * Possible values for a DoubleSolenoid.
+ */
+ public enum Value {
+ kOff,
+ kForward,
+ kReverse
+ }
+
+ private byte m_forwardMask; // The mask for the forward channel.
+ private byte m_reverseMask; // The mask for the reverse channel.
+ private int m_forwardHandle;
+ private int m_reverseHandle;
+
+ /**
+ * Constructor. Uses the default PCM ID (defaults to 0).
+ *
+ * @param forwardChannel The forward channel number on the PCM (0..7).
+ * @param reverseChannel The reverse channel number on the PCM (0..7).
+ */
+ public DoubleSolenoid(final int forwardChannel, final int reverseChannel) {
+ this(SensorUtil.getDefaultSolenoidModule(), forwardChannel, reverseChannel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The module number of the solenoid module to use.
+ * @param forwardChannel The forward channel on the module to control (0..7).
+ * @param reverseChannel The reverse channel on the module to control (0..7).
+ */
+ public DoubleSolenoid(final int moduleNumber, final int forwardChannel,
+ final int reverseChannel) {
+ super(moduleNumber);
+
+ SensorUtil.checkSolenoidModule(m_moduleNumber);
+ SensorUtil.checkSolenoidChannel(forwardChannel);
+ SensorUtil.checkSolenoidChannel(reverseChannel);
+
+ int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) forwardChannel);
+ m_forwardHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+
+ try {
+ portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) reverseChannel);
+ m_reverseHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+ } catch (UncleanStatusException ex) {
+ // free the forward handle on exception, then rethrow
+ SolenoidJNI.freeSolenoidPort(m_forwardHandle);
+ m_forwardHandle = 0;
+ m_reverseHandle = 0;
+ throw ex;
+ }
+
+ m_forwardMask = (byte) (1 << forwardChannel);
+ m_reverseMask = (byte) (1 << reverseChannel);
+
+ HAL.report(tResourceType.kResourceType_Solenoid, forwardChannel,
+ m_moduleNumber);
+ HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel,
+ m_moduleNumber);
+ setName("DoubleSolenoid", m_moduleNumber, forwardChannel);
+ }
+
+ @Override
+ public synchronized void close() {
+ super.close();
+ SolenoidJNI.freeSolenoidPort(m_forwardHandle);
+ SolenoidJNI.freeSolenoidPort(m_reverseHandle);
+ }
+
+ /**
+ * Set the value of a solenoid.
+ *
+ * @param value The value to set (Off, Forward, Reverse)
+ */
+ public void set(final Value value) {
+ boolean forward = false;
+ boolean reverse = false;
+
+ switch (value) {
+ case kOff:
+ forward = false;
+ reverse = false;
+ break;
+ case kForward:
+ forward = true;
+ reverse = false;
+ break;
+ case kReverse:
+ forward = false;
+ reverse = true;
+ break;
+ default:
+ throw new AssertionError("Illegal value: " + value);
+
+ }
+
+ SolenoidJNI.setSolenoid(m_forwardHandle, forward);
+ SolenoidJNI.setSolenoid(m_reverseHandle, reverse);
+ }
+
+ /**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+ public Value get() {
+ boolean valueForward = SolenoidJNI.getSolenoid(m_forwardHandle);
+ boolean valueReverse = SolenoidJNI.getSolenoid(m_reverseHandle);
+
+ if (valueForward) {
+ return Value.kForward;
+ }
+ if (valueReverse) {
+ return Value.kReverse;
+ }
+ return Value.kOff;
+ }
+
+ /**
+ * Check if the forward solenoid is blacklisted. If a solenoid is shorted, it is added to the
+ * blacklist and disabled until power cycle, or until faults are cleared.
+ *
+ * @return If solenoid is disabled due to short.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public boolean isFwdSolenoidBlackListed() {
+ int blackList = getPCMSolenoidBlackList();
+ return (blackList & m_forwardMask) != 0;
+ }
+
+ /**
+ * Check if the reverse solenoid is blacklisted. If a solenoid is shorted, it is added to the
+ * blacklist and disabled until power cycle, or until faults are cleared.
+ *
+ * @return If solenoid is disabled due to short.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public boolean isRevSolenoidBlackListed() {
+ int blackList = getPCMSolenoidBlackList();
+ return (blackList & m_reverseMask) != 0;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Double Solenoid");
+ builder.setActuator(true);
+ builder.setSafeState(() -> set(Value.kOff));
+ builder.addStringProperty("Value", () -> get().name().substring(1), value -> {
+ if ("Forward".equals(value)) {
+ set(Value.kForward);
+ } else if ("Reverse".equals(value)) {
+ set(Value.kReverse);
+ } else {
+ set(Value.kOff);
+ }
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
new file mode 100644
index 0000000..b5c46c9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -0,0 +1,1163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.hal.PowerJNI;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * Provide access to the network communication data to / from the Driver Station.
+ */
+@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.ExcessiveClassLength",
+ "PMD.ExcessivePublicCount", "PMD.GodClass", "PMD.TooManyFields",
+ "PMD.TooManyMethods"})
+public class DriverStation {
+ /**
+ * Number of Joystick Ports.
+ */
+ public static final int kJoystickPorts = 6;
+
+ private static class HALJoystickButtons {
+ public int m_buttons;
+ public byte m_count;
+ }
+
+ private static class HALJoystickAxes {
+ public float[] m_axes;
+ public short m_count;
+
+ HALJoystickAxes(int count) {
+ m_axes = new float[count];
+ }
+ }
+
+ private static class HALJoystickPOVs {
+ public short[] m_povs;
+ public short m_count;
+
+ HALJoystickPOVs(int count) {
+ m_povs = new short[count];
+ }
+ }
+
+ /**
+ * The robot alliance that the robot is a part of.
+ */
+ public enum Alliance {
+ Red, Blue, Invalid
+ }
+
+ public enum MatchType {
+ None, Practice, Qualification, Elimination
+ }
+
+ private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
+ private double m_nextMessageTime;
+
+ private static class DriverStationTask implements Runnable {
+ private final DriverStation m_ds;
+
+ DriverStationTask(DriverStation ds) {
+ m_ds = ds;
+ }
+
+ @Override
+ public void run() {
+ m_ds.run();
+ }
+ } /* DriverStationTask */
+
+ private static class MatchDataSender {
+ @SuppressWarnings("MemberName")
+ NetworkTable table;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry typeMetadata;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry gameSpecificMessage;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry eventName;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry matchNumber;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry replayNumber;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry matchType;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry alliance;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry station;
+ @SuppressWarnings("MemberName")
+ NetworkTableEntry controlWord;
+
+ MatchDataSender() {
+ table = NetworkTableInstance.getDefault().getTable("FMSInfo");
+ typeMetadata = table.getEntry(".type");
+ typeMetadata.forceSetString("FMSInfo");
+ gameSpecificMessage = table.getEntry("GameSpecificMessage");
+ gameSpecificMessage.forceSetString("");
+ eventName = table.getEntry("EventName");
+ eventName.forceSetString("");
+ matchNumber = table.getEntry("MatchNumber");
+ matchNumber.forceSetDouble(0);
+ replayNumber = table.getEntry("ReplayNumber");
+ replayNumber.forceSetDouble(0);
+ matchType = table.getEntry("MatchType");
+ matchType.forceSetDouble(0);
+ alliance = table.getEntry("IsRedAlliance");
+ alliance.forceSetBoolean(true);
+ station = table.getEntry("StationNumber");
+ station.forceSetDouble(1);
+ controlWord = table.getEntry("FMSControlData");
+ controlWord.forceSetDouble(0);
+ }
+ }
+
+ private static DriverStation instance = new DriverStation();
+
+ // Joystick User Data
+ private HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
+ private HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
+ private HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
+ private MatchInfoData m_matchInfo = new MatchInfoData();
+
+ // Joystick Cached Data
+ private HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
+ private HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
+ private HALJoystickButtons[] m_joystickButtonsCache = new HALJoystickButtons[kJoystickPorts];
+ private MatchInfoData m_matchInfoCache = new MatchInfoData();
+
+ // Joystick button rising/falling edge flags
+ private int[] m_joystickButtonsPressed = new int[kJoystickPorts];
+ private int[] m_joystickButtonsReleased = new int[kJoystickPorts];
+
+ // preallocated byte buffer for button count
+ private final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
+
+ private final MatchDataSender m_matchDataSender;
+
+ // Internal Driver Station thread
+ @SuppressWarnings("PMD.SingularField")
+ private final Thread m_thread;
+ private volatile boolean m_threadKeepAlive = true;
+
+ private final ReentrantLock m_cacheDataMutex = new ReentrantLock();
+
+ private final Lock m_waitForDataMutex;
+ private final Condition m_waitForDataCond;
+ private int m_waitForDataCount;
+
+ // Robot state status variables
+ private boolean m_userInDisabled;
+ private boolean m_userInAutonomous;
+ private boolean m_userInTeleop;
+ private boolean m_userInTest;
+
+ // Control word variables
+ private final Object m_controlWordMutex;
+ private final ControlWord m_controlWordCache;
+ private long m_lastControlWordUpdate;
+
+ /**
+ * Gets an instance of the DriverStation.
+ *
+ * @return The DriverStation.
+ */
+ public static DriverStation getInstance() {
+ return DriverStation.instance;
+ }
+
+ /**
+ * DriverStation constructor.
+ *
+ * <p>The single DriverStation instance is created statically with the instance static member
+ * variable.
+ */
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ private DriverStation() {
+ HAL.initialize(500, 0);
+ m_waitForDataCount = 0;
+ m_waitForDataMutex = new ReentrantLock();
+ m_waitForDataCond = m_waitForDataMutex.newCondition();
+
+ for (int i = 0; i < kJoystickPorts; i++) {
+ m_joystickButtons[i] = new HALJoystickButtons();
+ m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
+ m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
+
+ m_joystickButtonsCache[i] = new HALJoystickButtons();
+ m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
+ m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
+ }
+
+ m_controlWordMutex = new Object();
+ m_controlWordCache = new ControlWord();
+ m_lastControlWordUpdate = 0;
+
+ m_matchDataSender = new MatchDataSender();
+
+ m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
+ m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
+
+ m_thread.start();
+ }
+
+ /**
+ * Kill the thread.
+ */
+ public void release() {
+ m_threadKeepAlive = false;
+ }
+
+ /**
+ * Report error to Driver Station. Optionally appends Stack trace
+ * to error message.
+ *
+ * @param printTrace If true, append stack trace to error string
+ */
+ public static void reportError(String error, boolean printTrace) {
+ reportErrorImpl(true, 1, error, printTrace);
+ }
+
+ /**
+ * Report error to Driver Station. Appends provided stack trace
+ * to error message.
+ *
+ * @param stackTrace The stack trace to append
+ */
+ public static void reportError(String error, StackTraceElement[] stackTrace) {
+ reportErrorImpl(true, 1, error, stackTrace);
+ }
+
+ /**
+ * Report warning to Driver Station. Optionally appends Stack
+ * trace to warning message.
+ *
+ * @param printTrace If true, append stack trace to warning string
+ */
+ public static void reportWarning(String error, boolean printTrace) {
+ reportErrorImpl(false, 1, error, printTrace);
+ }
+
+ /**
+ * Report warning to Driver Station. Appends provided stack
+ * trace to warning message.
+ *
+ * @param stackTrace The stack trace to append
+ */
+ public static void reportWarning(String error, StackTraceElement[] stackTrace) {
+ reportErrorImpl(false, 1, error, stackTrace);
+ }
+
+ private static void reportErrorImpl(boolean isError, int code, String error, boolean
+ printTrace) {
+ reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3);
+ }
+
+ private static void reportErrorImpl(boolean isError, int code, String error,
+ StackTraceElement[] stackTrace) {
+ reportErrorImpl(isError, code, error, true, stackTrace, 0);
+ }
+
+ private static void reportErrorImpl(boolean isError, int code, String error,
+ boolean printTrace, StackTraceElement[] stackTrace, int stackTraceFirst) {
+ String locString;
+ if (stackTrace.length >= stackTraceFirst + 1) {
+ locString = stackTrace[stackTraceFirst].toString();
+ } else {
+ locString = "";
+ }
+ StringBuilder traceString = new StringBuilder("");
+ if (printTrace) {
+ boolean haveLoc = false;
+ for (int i = stackTraceFirst; i < stackTrace.length; i++) {
+ String loc = stackTrace[i].toString();
+ traceString.append("\tat ").append(loc).append('\n');
+ // get first user function
+ if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
+ locString = loc;
+ haveLoc = true;
+ }
+ }
+ }
+ HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
+ }
+
+ /**
+ * The state of one joystick button. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return The state of the joystick button.
+ */
+ public boolean getStickButton(final int stick, final int button) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+ if (button <= 0) {
+ reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+ return false;
+ }
+ m_cacheDataMutex.lock();
+ try {
+ if (button > m_joystickButtons[stick].m_count) {
+ // Unlock early so error printing isn't locked.
+ m_cacheDataMutex.unlock();
+ reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+
+ return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0;
+ } finally {
+ if (m_cacheDataMutex.isHeldByCurrentThread()) {
+ m_cacheDataMutex.unlock();
+ }
+ }
+ }
+
+ /**
+ * Whether one joystick button was pressed since the last check. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return Whether the joystick button was pressed since the last check.
+ */
+ boolean getStickButtonPressed(final int stick, final int button) {
+ if (button <= 0) {
+ reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+ return false;
+ }
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+ boolean error = false;
+ boolean retVal = false;
+ synchronized (m_cacheDataMutex) {
+ if (button > m_joystickButtons[stick].m_count) {
+ error = true;
+ retVal = false;
+ } else {
+ // If button was pressed, clear flag and return true
+ if ((m_joystickButtonsPressed[stick] & 1 << (button - 1)) != 0) {
+ m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
+ retVal = true;
+ } else {
+ retVal = false;
+ }
+ }
+ }
+ if (error) {
+ reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+ return retVal;
+ }
+
+ /**
+ * Whether one joystick button was released since the last check. Button indexes
+ * begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return Whether the joystick button was released since the last check.
+ */
+ boolean getStickButtonReleased(final int stick, final int button) {
+ if (button <= 0) {
+ reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+ return false;
+ }
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+ boolean error = false;
+ boolean retVal = false;
+ synchronized (m_cacheDataMutex) {
+ if (button > m_joystickButtons[stick].m_count) {
+ error = true;
+ retVal = false;
+ } else {
+ // If button was released, clear flag and return true
+ if ((m_joystickButtonsReleased[stick] & 1 << (button - 1)) != 0) {
+ m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
+ retVal = true;
+ } else {
+ retVal = false;
+ }
+ }
+ }
+ if (error) {
+ reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+ return retVal;
+ }
+
+ /**
+ * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected
+ * to the specified port.
+ *
+ * @param stick The joystick to read.
+ * @param axis The analog axis value to read from the joystick.
+ * @return The value of the axis on the joystick.
+ */
+ public double getStickAxis(int stick, int axis) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+ if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
+ throw new IllegalArgumentException("Joystick axis is out of range");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ if (axis >= m_joystickAxes[stick].m_count) {
+ // Unlock early so error printing isn't locked.
+ m_cacheDataMutex.unlock();
+ reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
+ + " not available, check if controller is plugged in");
+ return 0.0;
+ }
+
+ return m_joystickAxes[stick].m_axes[axis];
+ } finally {
+ if (m_cacheDataMutex.isHeldByCurrentThread()) {
+ m_cacheDataMutex.unlock();
+ }
+ }
+ }
+
+ /**
+ * Get the state of a POV on the joystick.
+ *
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+ public int getStickPOV(int stick, int pov) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+ if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
+ throw new IllegalArgumentException("Joystick POV is out of range");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ if (pov >= m_joystickPOVs[stick].m_count) {
+ // Unlock early so error printing isn't locked.
+ m_cacheDataMutex.unlock();
+ reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
+ + " not available, check if controller is plugged in");
+ }
+ } finally {
+ if (m_cacheDataMutex.isHeldByCurrentThread()) {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ return m_joystickPOVs[stick].m_povs[pov];
+ }
+
+ /**
+ * The state of the buttons on the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+ public int getStickButtons(final int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickButtons[stick].m_buttons;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the number of axes on a given joystick port.
+ *
+ * @param stick The joystick port number
+ * @return The number of axes on the indicated joystick
+ */
+ public int getStickAxisCount(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickAxes[stick].m_count;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the number of POVs on a given joystick port.
+ *
+ * @param stick The joystick port number
+ * @return The number of POVs on the indicated joystick
+ */
+ public int getStickPOVCount(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickPOVs[stick].m_count;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Gets the number of buttons on a joystick.
+ *
+ * @param stick The joystick port number
+ * @return The number of buttons on the indicated joystick
+ */
+ public int getStickButtonCount(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ m_cacheDataMutex.lock();
+ try {
+ return m_joystickButtons[stick].m_count;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Gets the value of isXbox on a joystick.
+ *
+ * @param stick The joystick port number
+ * @return A boolean that returns the value of isXbox
+ */
+ public boolean getJoystickIsXbox(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickIsXbox((byte) stick) == 1;
+ }
+
+ /**
+ * Gets the value of type on a joystick.
+ *
+ * @param stick The joystick port number
+ * @return The value of type
+ */
+ public int getJoystickType(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickType((byte) stick);
+ }
+
+ /**
+ * Gets the name of the joystick at a port.
+ *
+ * @param stick The joystick port number
+ * @return The value of name
+ */
+ public String getJoystickName(int stick) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickName((byte) stick);
+ }
+
+ /**
+ * Returns the types of Axes on a given joystick port.
+ *
+ * @param stick The joystick port number
+ * @param axis The target axis
+ * @return What type of axis the axis is reporting to be
+ */
+ public int getJoystickAxisType(int stick, int axis) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+ }
+
+ return HAL.getJoystickAxisType((byte) stick, (byte) axis);
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be enabled.
+ *
+ * @return True if the robot is enabled, false otherwise.
+ */
+ public boolean isEnabled() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be disabled.
+ *
+ * @return True if the robot should be disabled, false otherwise.
+ */
+ public boolean isDisabled() {
+ return !isEnabled();
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be running in
+ * autonomous mode.
+ *
+ * @return True if autonomous mode should be enabled, false otherwise.
+ */
+ public boolean isAutonomous() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getAutonomous();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be running in
+ * operator-controlled mode.
+ *
+ * @return True if operator-controlled mode should be enabled, false otherwise.
+ */
+ public boolean isOperatorControl() {
+ return !(isAutonomous() || isTest());
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station requires the robot to be running in test
+ * mode.
+ *
+ * @return True if test mode should be enabled, false otherwise.
+ */
+ public boolean isTest() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getTest();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the Driver Station is attached.
+ *
+ * @return True if Driver Station is attached, false otherwise.
+ */
+ public boolean isDSAttached() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getDSAttached();
+ }
+ }
+
+ /**
+ * Gets if a new control packet from the driver station arrived since the last time this function
+ * was called.
+ *
+ * @return True if the control data has been updated since the last call.
+ */
+ public boolean isNewControlData() {
+ return HAL.isNewControlData();
+ }
+
+ /**
+ * Gets if the driver station attached to a Field Management System.
+ *
+ * @return true if the robot is competing on a field being controlled by a Field Management System
+ */
+ public boolean isFMSAttached() {
+ synchronized (m_controlWordMutex) {
+ updateControlWord(false);
+ return m_controlWordCache.getFMSAttached();
+ }
+ }
+
+ /**
+ * Gets a value indicating whether the FPGA outputs are enabled. The outputs may be disabled if
+ * the robot is disabled or e-stopped, the watchdog has expired, or if the roboRIO browns out.
+ *
+ * @return True if the FPGA outputs are enabled.
+ * @deprecated Use RobotController.isSysActive()
+ */
+ @Deprecated
+ public boolean isSysActive() {
+ return HAL.getSystemActive();
+ }
+
+ /**
+ * Check if the system is browned out.
+ *
+ * @return True if the system is browned out
+ * @deprecated Use RobotController.isBrownedOut()
+ */
+ @Deprecated
+ public boolean isBrownedOut() {
+ return HAL.getBrownedOut();
+ }
+
+ /**
+ * Get the game specific message.
+ *
+ * @return the game specific message
+ */
+ public String getGameSpecificMessage() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.gameSpecificMessage;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the event name.
+ *
+ * @return the event name
+ */
+ public String getEventName() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.eventName;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the match type.
+ *
+ * @return the match type
+ */
+ public MatchType getMatchType() {
+ int matchType;
+ m_cacheDataMutex.lock();
+ try {
+ matchType = m_matchInfo.matchType;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ switch (matchType) {
+ case 1:
+ return MatchType.Practice;
+ case 2:
+ return MatchType.Qualification;
+ case 3:
+ return MatchType.Elimination;
+ default:
+ return MatchType.None;
+ }
+ }
+
+ /**
+ * Get the match number.
+ *
+ * @return the match number
+ */
+ public int getMatchNumber() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.matchNumber;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the replay number.
+ *
+ * @return the replay number
+ */
+ public int getReplayNumber() {
+ m_cacheDataMutex.lock();
+ try {
+ return m_matchInfo.replayNumber;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the current alliance from the FMS.
+ *
+ * @return the current alliance
+ */
+ public Alliance getAlliance() {
+ AllianceStationID allianceStationID = HAL.getAllianceStation();
+ if (allianceStationID == null) {
+ return Alliance.Invalid;
+ }
+
+ switch (allianceStationID) {
+ case Red1:
+ case Red2:
+ case Red3:
+ return Alliance.Red;
+
+ case Blue1:
+ case Blue2:
+ case Blue3:
+ return Alliance.Blue;
+
+ default:
+ return Alliance.Invalid;
+ }
+ }
+
+ /**
+ * Gets the location of the team's driver station controls.
+ *
+ * @return the location of the team's driver station controls: 1, 2, or 3
+ */
+ public int getLocation() {
+ AllianceStationID allianceStationID = HAL.getAllianceStation();
+ if (allianceStationID == null) {
+ return 0;
+ }
+ switch (allianceStationID) {
+ case Red1:
+ case Blue1:
+ return 1;
+
+ case Red2:
+ case Blue2:
+ return 2;
+
+ case Blue3:
+ case Red3:
+ return 3;
+
+ default:
+ return 0;
+ }
+ }
+
+ /**
+ * Wait for new data from the driver station.
+ */
+ public void waitForData() {
+ waitForData(0);
+ }
+
+ /**
+ * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
+ * only.
+ *
+ * @param timeout The maximum time in seconds to wait.
+ * @return true if there is new data, otherwise false
+ */
+ public boolean waitForData(double timeout) {
+ long startTime = RobotController.getFPGATime();
+ long timeoutMicros = (long) (timeout * 1000000);
+ m_waitForDataMutex.lock();
+ try {
+ int currentCount = m_waitForDataCount;
+ while (m_waitForDataCount == currentCount) {
+ if (timeout > 0) {
+ long now = RobotController.getFPGATime();
+ if (now < startTime + timeoutMicros) {
+ // We still have time to wait
+ boolean signaled = m_waitForDataCond.await(startTime + timeoutMicros - now,
+ TimeUnit.MICROSECONDS);
+ if (!signaled) {
+ // Return false if a timeout happened
+ return false;
+ }
+ } else {
+ // Time has elapsed.
+ return false;
+ }
+ } else {
+ m_waitForDataCond.await();
+ }
+ }
+ // Return true if we have received a proper signal
+ return true;
+ } catch (InterruptedException ex) {
+ // return false on a thread interrupt
+ return false;
+ } finally {
+ m_waitForDataMutex.unlock();
+ }
+ }
+
+ /**
+ * Return the approximate match time. The FMS does not send an official match time to the robots,
+ * but does send an approximate match time. The value will count down the time remaining in the
+ * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
+ * dispute ref calls or guarantee that a function will trigger before the match ends) The
+ * Practice Match function of the DS approximates the behaviour seen on the field.
+ *
+ * @return Time remaining in current match period (auto or teleop) in seconds
+ */
+ public double getMatchTime() {
+ return HAL.getMatchTime();
+ }
+
+ /**
+ * Read the battery voltage.
+ *
+ * @return The battery voltage in Volts.
+ * @deprecated Use RobotController.getBatteryVoltage
+ */
+ @Deprecated
+ public double getBatteryVoltage() {
+ return PowerJNI.getVinVoltage();
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting disabled code; if false, leaving disabled code
+ */
+ @SuppressWarnings("MethodName")
+ public void InDisabled(boolean entering) {
+ m_userInDisabled = entering;
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting autonomous code; if false, leaving autonomous code
+ */
+ @SuppressWarnings("MethodName")
+ public void InAutonomous(boolean entering) {
+ m_userInAutonomous = entering;
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting teleop code; if false, leaving teleop code
+ */
+ @SuppressWarnings("MethodName")
+ public void InOperatorControl(boolean entering) {
+ m_userInTeleop = entering;
+ }
+
+ /**
+ * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+ * purposes only.
+ *
+ * @param entering If true, starting test code; if false, leaving test code
+ */
+ @SuppressWarnings("MethodName")
+ public void InTest(boolean entering) {
+ m_userInTest = entering;
+ }
+
+ private void sendMatchData() {
+ AllianceStationID alliance = HAL.getAllianceStation();
+ boolean isRedAlliance = false;
+ int stationNumber = 1;
+ switch (alliance) {
+ case Blue1:
+ isRedAlliance = false;
+ stationNumber = 1;
+ break;
+ case Blue2:
+ isRedAlliance = false;
+ stationNumber = 2;
+ break;
+ case Blue3:
+ isRedAlliance = false;
+ stationNumber = 3;
+ break;
+ case Red1:
+ isRedAlliance = true;
+ stationNumber = 1;
+ break;
+ case Red2:
+ isRedAlliance = true;
+ stationNumber = 2;
+ break;
+ default:
+ isRedAlliance = true;
+ stationNumber = 3;
+ break;
+ }
+
+
+ String eventName;
+ String gameSpecificMessage;
+ int matchNumber;
+ int replayNumber;
+ int matchType;
+ synchronized (m_cacheDataMutex) {
+ eventName = m_matchInfo.eventName;
+ gameSpecificMessage = m_matchInfo.gameSpecificMessage;
+ matchNumber = m_matchInfo.matchNumber;
+ replayNumber = m_matchInfo.replayNumber;
+ matchType = m_matchInfo.matchType;
+ }
+
+ m_matchDataSender.alliance.setBoolean(isRedAlliance);
+ m_matchDataSender.station.setDouble(stationNumber);
+ m_matchDataSender.eventName.setString(eventName);
+ m_matchDataSender.gameSpecificMessage.setString(gameSpecificMessage);
+ m_matchDataSender.matchNumber.setDouble(matchNumber);
+ m_matchDataSender.replayNumber.setDouble(replayNumber);
+ m_matchDataSender.matchType.setDouble(matchType);
+ m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord());
+ }
+
+ /**
+ * Copy data from the DS task for the user. If no new data exists, it will just be returned,
+ * otherwise the data will be copied from the DS polling loop.
+ */
+ protected void getData() {
+ // Get the status of all of the joysticks
+ for (byte stick = 0; stick < kJoystickPorts; stick++) {
+ m_joystickAxesCache[stick].m_count =
+ HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
+ m_joystickPOVsCache[stick].m_count =
+ HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
+ m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
+ m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
+ }
+
+ HAL.getMatchInfo(m_matchInfoCache);
+
+ // Force a control word update, to make sure the data is the newest.
+ updateControlWord(true);
+
+ // lock joystick mutex to swap cache data
+ m_cacheDataMutex.lock();
+ try {
+ for (int i = 0; i < kJoystickPorts; i++) {
+ // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+ m_joystickButtonsPressed[i] |=
+ ~m_joystickButtons[i].m_buttons & m_joystickButtonsCache[i].m_buttons;
+
+ // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+ m_joystickButtonsReleased[i] |=
+ m_joystickButtons[i].m_buttons & ~m_joystickButtonsCache[i].m_buttons;
+ }
+
+ // move cache to actual data
+ HALJoystickAxes[] currentAxes = m_joystickAxes;
+ m_joystickAxes = m_joystickAxesCache;
+ m_joystickAxesCache = currentAxes;
+
+ HALJoystickButtons[] currentButtons = m_joystickButtons;
+ m_joystickButtons = m_joystickButtonsCache;
+ m_joystickButtonsCache = currentButtons;
+
+ HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
+ m_joystickPOVs = m_joystickPOVsCache;
+ m_joystickPOVsCache = currentPOVs;
+
+ MatchInfoData currentInfo = m_matchInfo;
+ m_matchInfo = m_matchInfoCache;
+ m_matchInfoCache = currentInfo;
+ } finally {
+ m_cacheDataMutex.unlock();
+ }
+
+ m_waitForDataMutex.lock();
+ m_waitForDataCount++;
+ m_waitForDataCond.signalAll();
+ m_waitForDataMutex.unlock();
+
+ sendMatchData();
+ }
+
+ /**
+ * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+ * the DS.
+ */
+ private void reportJoystickUnpluggedError(String message) {
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ reportError(message, false);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+ }
+
+ /**
+ * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+ * the DS.
+ */
+ private void reportJoystickUnpluggedWarning(String message) {
+ double currentTime = Timer.getFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ reportWarning(message, false);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+ }
+
+ /**
+ * Provides the service routine for the DS polling m_thread.
+ */
+ private void run() {
+ int safetyCounter = 0;
+ while (m_threadKeepAlive) {
+ HAL.waitForDSData();
+ getData();
+
+ if (isDisabled()) {
+ safetyCounter = 0;
+ }
+
+ safetyCounter++;
+ if (safetyCounter >= 4) {
+ MotorSafety.checkMotors();
+ safetyCounter = 0;
+ }
+ if (m_userInDisabled) {
+ HAL.observeUserProgramDisabled();
+ }
+ if (m_userInAutonomous) {
+ HAL.observeUserProgramAutonomous();
+ }
+ if (m_userInTeleop) {
+ HAL.observeUserProgramTeleop();
+ }
+ if (m_userInTest) {
+ HAL.observeUserProgramTest();
+ }
+ }
+ }
+
+ /**
+ * Updates the data in the control word cache. Updates if the force parameter is set, or if
+ * 50ms have passed since the last update.
+ *
+ * @param force True to force an update to the cache, otherwise update if 50ms have passed.
+ */
+ private void updateControlWord(boolean force) {
+ long now = System.currentTimeMillis();
+ synchronized (m_controlWordMutex) {
+ if (now - m_lastControlWordUpdate > 50 || force) {
+ HAL.getControlWord(m_controlWordCache);
+ m_lastControlWordUpdate = now;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
new file mode 100644
index 0000000..594fe0a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -0,0 +1,575 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.EncoderJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Class to read quadrature encoders.
+ *
+ * <p>Quadrature encoders are devices that count shaft rotation and can sense direction. The output
+ * of the Encoder class is an integer that can count either up or down, and can go negative for
+ * reverse direction counting. When creating Encoders, a direction can be supplied that inverts the
+ * sense of the output to make code more readable if the encoder is mounted such that forward
+ * movement generates negative values. Quadrature encoders have two digital outputs, an A Channel
+ * and a B Channel, that are out of phase with each other for direction sensing.
+ *
+ * <p>All encoders will immediately start counting - reset() them if you need them to be zeroed
+ * before use.
+ */
+public class Encoder extends SendableBase implements CounterBase, PIDSource {
+ public enum IndexingType {
+ kResetWhileHigh(0), kResetWhileLow(1), kResetOnFallingEdge(2), kResetOnRisingEdge(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ IndexingType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * The a source.
+ */
+ @SuppressWarnings("MemberName")
+ protected DigitalSource m_aSource; // the A phase of the quad encoder
+ /**
+ * The b source.
+ */
+ @SuppressWarnings("MemberName")
+ protected DigitalSource m_bSource; // the B phase of the quad encoder
+ /**
+ * The index source.
+ */
+ protected DigitalSource m_indexSource; // Index on some encoders
+ private boolean m_allocatedA;
+ private boolean m_allocatedB;
+ private boolean m_allocatedI;
+ private PIDSourceType m_pidSource;
+
+ private int m_encoder; // the HAL encoder object
+
+
+ /**
+ * Common initialization code for Encoders. This code allocates resources for Encoders and is
+ * common to all constructors.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param reverseDirection If true, counts down instead of up (this is all relative)
+ */
+ private void initEncoder(boolean reverseDirection, final EncodingType type) {
+ m_encoder = EncoderJNI.initializeEncoder(m_aSource.getPortHandleForRouting(),
+ m_aSource.getAnalogTriggerTypeForRouting(), m_bSource.getPortHandleForRouting(),
+ m_bSource.getAnalogTriggerTypeForRouting(), reverseDirection, type.value);
+
+ m_pidSource = PIDSourceType.kDisplacement;
+
+ int fpgaIndex = getFPGAIndex();
+ HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex, type.value);
+ setName("Encoder", fpgaIndex);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+ * @param channelB The b channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(final int channelA, final int channelB, boolean reverseDirection) {
+ this(channelA, channelB, reverseDirection, EncodingType.k4X);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ */
+ public Encoder(final int channelA, final int channelB) {
+ this(channelA, channelB, false);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts
+ * will be 4x the encoder spec'd value since all rising and falling edges
+ * are counted. If 1X or 2X are selected then a m_counter object will be
+ * used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+ public Encoder(final int channelA, final int channelB, boolean reverseDirection,
+ final EncodingType encodingType) {
+ requireNonNull(encodingType, "Given encoding type was null");
+
+ m_allocatedA = true;
+ m_allocatedB = true;
+ m_allocatedI = false;
+ m_aSource = new DigitalInput(channelA);
+ m_bSource = new DigitalInput(channelB);
+ addChild(m_aSource);
+ addChild(m_bSource);
+ initEncoder(reverseDirection, encodingType);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels. Using an index pulse forces 4x
+ * encoding
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ * @param indexChannel The index channel digital input channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(final int channelA, final int channelB, final int indexChannel,
+ boolean reverseDirection) {
+ this(channelA, channelB, reverseDirection);
+ m_allocatedI = true;
+ m_indexSource = new DigitalInput(indexChannel);
+ addChild(m_indexSource);
+ setIndexSource(m_indexSource);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels. Using an index pulse forces 4x
+ * encoding
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param channelA The a channel digital input channel.
+ * @param channelB The b channel digital input channel.
+ * @param indexChannel The index channel digital input channel.
+ */
+ public Encoder(final int channelA, final int channelB, final int indexChannel) {
+ this(channelA, channelB, indexChannel, false);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels as digital inputs. This is used
+ * in the case where the digital inputs are shared. The Encoder class will not allocate the
+ * digital inputs and assume that they already are counted.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) {
+ this(sourceA, sourceB, reverseDirection, EncodingType.k4X);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels as digital inputs. This is used
+ * in the case where the digital inputs are shared. The Encoder class will not allocate the
+ * digital inputs and assume that they already are counted.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB) {
+ this(sourceA, sourceB, false);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a and b channels as digital inputs. This is used
+ * in the case where the digital inputs are shared. The Encoder class will not allocate the
+ * digital inputs and assume that they already are counted.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts
+ * will be 4x the encoder spec'd value since all rising and falling edges
+ * are counted. If 1X or 2X are selected then a m_counter object will be
+ * used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection,
+ final EncodingType encodingType) {
+ requireNonNull(sourceA, "Digital Source A was null");
+ requireNonNull(sourceB, "Digital Source B was null");
+ requireNonNull(encodingType, "Given encoding type was null");
+
+ m_allocatedA = false;
+ m_allocatedB = false;
+ m_allocatedI = false;
+ m_aSource = sourceA;
+ m_bSource = sourceB;
+ initEncoder(reverseDirection, encodingType);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a, b and index channels as digital inputs. This
+ * is used in the case where the digital inputs are shared. The Encoder class will not allocate
+ * the digital inputs and assume that they already are counted.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param indexSource the source that should be used for the index channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource,
+ boolean reverseDirection) {
+ this(sourceA, sourceB, reverseDirection);
+ m_allocatedI = false;
+ m_indexSource = indexSource;
+ setIndexSource(indexSource);
+ }
+
+ /**
+ * Encoder constructor. Construct a Encoder given a, b and index channels as digital inputs. This
+ * is used in the case where the digital inputs are shared. The Encoder class will not allocate
+ * the digital inputs and assume that they already are counted.
+ *
+ * <p>The encoder will start counting immediately.
+ *
+ * @param sourceA The source that should be used for the a channel.
+ * @param sourceB the source that should be used for the b channel.
+ * @param indexSource the source that should be used for the index channel.
+ */
+ public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) {
+ this(sourceA, sourceB, indexSource, false);
+ }
+
+ /**
+ * Get the FPGA index of the encoder.
+ *
+ * @return The Encoder's FPGA index.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public int getFPGAIndex() {
+ return EncoderJNI.getEncoderFPGAIndex(m_encoder);
+ }
+
+ /**
+ * Used to divide raw edge counts down to spec'd counts.
+ *
+ * @return The encoding scale factor 1x, 2x, or 4x, per the requested encoding type.
+ */
+ public int getEncodingScale() {
+ return EncoderJNI.getEncoderEncodingScale(m_encoder);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ if (m_aSource != null && m_allocatedA) {
+ m_aSource.close();
+ m_allocatedA = false;
+ }
+ if (m_bSource != null && m_allocatedB) {
+ m_bSource.close();
+ m_allocatedB = false;
+ }
+ if (m_indexSource != null && m_allocatedI) {
+ m_indexSource.close();
+ m_allocatedI = false;
+ }
+
+ m_aSource = null;
+ m_bSource = null;
+ m_indexSource = null;
+ EncoderJNI.freeEncoder(m_encoder);
+ m_encoder = 0;
+ }
+
+ /**
+ * Gets the raw value from the encoder. The raw value is the actual count unscaled by the 1x, 2x,
+ * or 4x scale factor.
+ *
+ * @return Current raw count from the encoder
+ */
+ public int getRaw() {
+ return EncoderJNI.getEncoderRaw(m_encoder);
+ }
+
+ /**
+ * Gets the current count. Returns the current count on the Encoder. This method compensates for
+ * the decoding type.
+ *
+ * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
+ */
+ @Override
+ public int get() {
+ return EncoderJNI.getEncoder(m_encoder);
+ }
+
+ /**
+ * Reset the Encoder distance to zero. Resets the current count to zero on the encoder.
+ */
+ @Override
+ public void reset() {
+ EncoderJNI.resetEncoder(m_encoder);
+ }
+
+ /**
+ * Returns the period of the most recent pulse. Returns the period of the most recent Encoder
+ * pulse in seconds. This method compensates for the decoding type.
+ *
+ * <p><b>Warning:</b> This returns unscaled periods. Use getRate() for rates that are scaled using
+ * the value from setDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ * @deprecated Use getRate() in favor of this method.
+ */
+ @Override
+ @Deprecated
+ public double getPeriod() {
+ return EncoderJNI.getEncoderPeriod(m_encoder);
+ }
+
+ /**
+ * Sets the maximum period for stopped detection. Sets the value that represents the maximum
+ * period of the Encoder before it will assume that the attached device is stopped. This timeout
+ * allows users to determine if the wheels or other shaft has stopped rotating. This method
+ * compensates for the decoding type.
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the FPGA will report
+ * the device stopped. This is expressed in seconds.
+ */
+ @Override
+ public void setMaxPeriod(double maxPeriod) {
+ EncoderJNI.setEncoderMaxPeriod(m_encoder, maxPeriod);
+ }
+
+ /**
+ * Determine if the encoder is stopped. Using the MaxPeriod value, a boolean is returned that is
+ * true if the encoder is considered stopped and false if it is still moving. A stopped encoder is
+ * one where the most recent pulse width exceeds the MaxPeriod.
+ *
+ * @return True if the encoder is considered stopped.
+ */
+ @Override
+ public boolean getStopped() {
+ return EncoderJNI.getEncoderStopped(m_encoder);
+ }
+
+ /**
+ * The last direction the encoder value changed.
+ *
+ * @return The last direction the encoder value changed.
+ */
+ @Override
+ public boolean getDirection() {
+ return EncoderJNI.getEncoderDirection(m_encoder);
+ }
+
+ /**
+ * Get the distance the robot has driven since the last reset as scaled by the value from {@link
+ * #setDistancePerPulse(double)}.
+ *
+ * @return The distance driven since the last reset
+ */
+ public double getDistance() {
+ return EncoderJNI.getEncoderDistance(m_encoder);
+ }
+
+ /**
+ * Get the current rate of the encoder. Units are distance per second as scaled by the value from
+ * setDistancePerPulse().
+ *
+ * @return The current rate of the encoder.
+ */
+ public double getRate() {
+ return EncoderJNI.getEncoderRate(m_encoder);
+ }
+
+ /**
+ * Set the minimum rate of the device before the hardware reports it stopped.
+ *
+ * @param minRate The minimum rate. The units are in distance per second as scaled by the value
+ * from setDistancePerPulse().
+ */
+ public void setMinRate(double minRate) {
+ EncoderJNI.setEncoderMinRate(m_encoder, minRate);
+ }
+
+ /**
+ * Set the distance per pulse for this encoder. This sets the multiplier used to determine the
+ * distance driven based on the count value from the encoder. Do not include the decoding type in
+ * this scale. The library already compensates for the decoding type. Set this value based on the
+ * encoder's rated Pulses per Revolution and factor in gearing reductions following the encoder
+ * shaft. This distance can be in any units you like, linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+ */
+ public void setDistancePerPulse(double distancePerPulse) {
+ EncoderJNI.setEncoderDistancePerPulse(m_encoder, distancePerPulse);
+ }
+
+ /**
+ * Get the distance per pulse for this encoder.
+ *
+ * @return The scale factor that will be used to convert pulses to useful units.
+ */
+ public double getDistancePerPulse() {
+ return EncoderJNI.getEncoderDistancePerPulse(m_encoder);
+ }
+
+ /**
+ * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so
+ * that it could count in the correct software direction regardless of the mounting.
+ *
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+ public void setReverseDirection(boolean reverseDirection) {
+ EncoderJNI.setEncoderReverseDirection(m_encoder, reverseDirection);
+ }
+
+ /**
+ * Set the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+ public void setSamplesToAverage(int samplesToAverage) {
+ EncoderJNI.setEncoderSamplesToAverage(m_encoder, samplesToAverage);
+ }
+
+ /**
+ * Get the Samples to Average which specifies the number of samples of the timer to average when
+ * calculating the period. Perform averaging to account for mechanical imperfections or as
+ * oversampling to increase resolution.
+ *
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+ public int getSamplesToAverage() {
+ return EncoderJNI.getEncoderSamplesToAverage(m_encoder);
+ }
+
+ /**
+ * Set which parameter of the encoder you are using as a process control variable. The encoder
+ * class supports the rate and distance parameters.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Implement the PIDSource interface.
+ *
+ * @return The current value of the selected source parameter.
+ */
+ @Override
+ public double pidGet() {
+ switch (m_pidSource) {
+ case kDisplacement:
+ return getDistance();
+ case kRate:
+ return getRate();
+ default:
+ return 0.0;
+ }
+ }
+
+ /**
+ * Set the index source for the encoder. When this source is activated, the encoder count
+ * automatically resets.
+ *
+ * @param channel A DIO channel to set as the encoder index
+ */
+ public void setIndexSource(int channel) {
+ setIndexSource(channel, IndexingType.kResetOnRisingEdge);
+ }
+
+ /**
+ * Set the index source for the encoder. When this source is activated, the encoder count
+ * automatically resets.
+ *
+ * @param source A digital source to set as the encoder index
+ */
+ public void setIndexSource(DigitalSource source) {
+ setIndexSource(source, IndexingType.kResetOnRisingEdge);
+ }
+
+ /**
+ * Set the index source for the encoder. When this source rises, the encoder count automatically
+ * resets.
+ *
+ * @param channel A DIO channel to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+ public void setIndexSource(int channel, IndexingType type) {
+ if (m_allocatedI) {
+ throw new AllocationException("Digital Input for Indexing already allocated");
+ }
+ m_indexSource = new DigitalInput(channel);
+ m_allocatedI = true;
+ addChild(m_indexSource);
+ setIndexSource(m_indexSource, type);
+ }
+
+ /**
+ * Set the index source for the encoder. When this source rises, the encoder count automatically
+ * resets.
+ *
+ * @param source A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+ public void setIndexSource(DigitalSource source, IndexingType type) {
+ EncoderJNI.setEncoderIndexSource(m_encoder, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting(), type.value);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) {
+ builder.setSmartDashboardType("Quadrature Encoder");
+ } else {
+ builder.setSmartDashboardType("Encoder");
+ }
+
+ builder.addDoubleProperty("Speed", this::getRate, null);
+ builder.addDoubleProperty("Distance", this::getDistance, null);
+ builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
new file mode 100644
index 0000000..cfefb6a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.File;
+
+/**
+ * Class for interacting with the Filesystem, particularly, interacting with
+ * FRC-related paths on the system, such as the launch and deploy directories.
+ *
+ * <p>This class is primarily used for obtaining resources in src/main/deploy, and
+ * the RoboRIO path /home/lvuser in a simulation-compatible way.</p>
+ */
+public final class Filesystem {
+ private Filesystem() { }
+
+ /**
+ * Obtains the current working path that the program was launched with.
+ * This is analogous to the `pwd` command on unix.
+ *
+ * @return The current working directory (launch directory)
+ */
+ public static File getLaunchDirectory() {
+ return new File(System.getProperty("user.dir")).getAbsoluteFile();
+ }
+
+ /**
+ * Obtains the operating directory of the program. On the roboRIO, this is
+ * /home/lvuser. In simulation, it is where the simulation was launched from
+ * (`pwd`).
+ *
+ * @return The operating directory
+ */
+ public static File getOperatingDirectory() {
+ if (RobotBase.isReal()) {
+ return new File("/home/lvuser");
+ } else {
+ return getLaunchDirectory();
+ }
+ }
+
+ /**
+ * Obtains the deploy directory of the program, which is the remote location
+ * src/main/deploy is deployed to by default. On the roboRIO, this is
+ * /home/lvuser/deploy. In simulation, it is where the simulation was launched
+ * from, in the subdirectory "deploy" (`pwd`/deploy).
+ *
+ * @return The deploy directory
+ */
+ public static File getDeployDirectory() {
+ return new File(getOperatingDirectory(), "deploy");
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GamepadBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GamepadBase.java
new file mode 100644
index 0000000..8e878ea
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GamepadBase.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Gamepad Interface.
+ *
+ * @deprecated Inherit directly from GenericHID instead.
+ */
+@Deprecated
+public abstract class GamepadBase extends GenericHID {
+ public GamepadBase(int port) {
+ super(port);
+ }
+
+ @Override
+ public abstract double getRawAxis(int axis);
+
+ /**
+ * Is the bumper pressed.
+ *
+ * @param hand which hand
+ * @return true if the bumper is pressed
+ */
+ public abstract boolean getBumper(Hand hand);
+
+ /**
+ * Is the bumper pressed.
+ *
+ * @return true if the bumper is pressed
+ */
+ public boolean getBumper() {
+ return getBumper(Hand.kRight);
+ }
+
+ public abstract boolean getStickButton(Hand hand);
+
+ public boolean getStickButton() {
+ return getStickButton(Hand.kRight);
+ }
+
+ @Override
+ public abstract boolean getRawButton(int button);
+
+ @Override
+ public abstract int getPOV(int pov);
+
+ @Override
+ public abstract int getPOVCount();
+
+ @Override
+ public abstract HIDType getType();
+
+ @Override
+ public abstract String getName();
+
+ @Override
+ public abstract void setOutput(int outputNumber, boolean value);
+
+ @Override
+ public abstract void setOutputs(int value);
+
+ @Override
+ public abstract void setRumble(RumbleType type, double value);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
new file mode 100644
index 0000000..e937c26
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Alias for counter class. Implement the gear tooth sensor supplied by FIRST. Currently there is no
+ * reverse sensing on the gear tooth sensor, but in future versions we might implement the necessary
+ * timing in the FPGA to sense direction.
+ */
+public class GearTooth extends Counter {
+ private static final double kGearToothThreshold = 55e-6;
+
+ /**
+ * Common code called by the constructors.
+ */
+ public void enableDirectionSensing(boolean directionSensitive) {
+ if (directionSensitive) {
+ setPulseLengthMode(kGearToothThreshold);
+ }
+ }
+
+ /**
+ * Construct a GearTooth sensor given a channel.
+ *
+ * <p>No direction sensing is assumed.
+ *
+ * @param channel The GPIO channel that the sensor is connected to.
+ */
+ public GearTooth(final int channel) {
+ this(channel, false);
+ }
+
+ /**
+ * Construct a GearTooth sensor given a channel.
+ *
+ * @param channel The DIO channel that the sensor is connected to. 0-9 are on-board,
+ * 10-25 are on the MXP port
+ * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
+ * direction.
+ */
+ public GearTooth(final int channel, boolean directionSensitive) {
+ super(channel);
+ enableDirectionSensing(directionSensitive);
+ if (directionSensitive) {
+ HAL.report(tResourceType.kResourceType_GearTooth, channel, 0, "D");
+ } else {
+ HAL.report(tResourceType.kResourceType_GearTooth, channel, 0);
+ }
+ setName("GearTooth", channel);
+ }
+
+ /**
+ * Construct a GearTooth sensor given a digital input. This should be used when sharing digital
+ * inputs.
+ *
+ * @param source An existing DigitalSource object (such as a DigitalInput)
+ * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
+ * direction.
+ */
+ public GearTooth(DigitalSource source, boolean directionSensitive) {
+ super(source);
+ enableDirectionSensing(directionSensitive);
+ if (directionSensitive) {
+ HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel(), 0, "D");
+ } else {
+ HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel(), 0);
+ }
+ setName("GearTooth", source.getChannel());
+ }
+
+ /**
+ * Construct a GearTooth sensor given a digital input. This should be used when sharing digital
+ * inputs.
+ *
+ * <p>No direction sensing is assumed.
+ *
+ * @param source An object that fully describes the input that the sensor is connected to.
+ */
+ public GearTooth(DigitalSource source) {
+ this(source, false);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ super.initSendable(builder);
+ builder.setSmartDashboardType("Gear Tooth");
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
new file mode 100644
index 0000000..0f884b9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
@@ -0,0 +1,286 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import edu.wpi.first.hal.HAL;
+
+/**
+ * GenericHID Interface.
+ */
+public abstract class GenericHID {
+ /**
+ * Represents a rumble output on the JoyStick.
+ */
+ public enum RumbleType {
+ kLeftRumble, kRightRumble
+ }
+
+ public enum HIDType {
+ kUnknown(-1),
+ kXInputUnknown(0),
+ kXInputGamepad(1),
+ kXInputWheel(2),
+ kXInputArcadeStick(3),
+ kXInputFlightStick(4),
+ kXInputDancePad(5),
+ kXInputGuitar(6),
+ kXInputGuitar2(7),
+ kXInputDrumKit(8),
+ kXInputGuitar3(11),
+ kXInputArcadePad(19),
+ kHIDJoystick(20),
+ kHIDGamepad(21),
+ kHIDDriving(22),
+ kHIDFlight(23),
+ kHID1stPerson(24);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private static final Map<Integer, HIDType> map = new HashMap<>();
+
+ HIDType(int value) {
+ this.value = value;
+ }
+
+ static {
+ for (HIDType hidType : HIDType.values()) {
+ map.put(hidType.value, hidType);
+ }
+ }
+
+ public static HIDType of(int value) {
+ return map.get(value);
+ }
+ }
+
+ /**
+ * Which hand the Human Interface Device is associated with.
+ */
+ public enum Hand {
+ kLeft(0), kRight(1);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Hand(int value) {
+ this.value = value;
+ }
+ }
+
+ private DriverStation m_ds;
+ private final int m_port;
+ private int m_outputs;
+ private short m_leftRumble;
+ private short m_rightRumble;
+
+ public GenericHID(int port) {
+ m_ds = DriverStation.getInstance();
+ m_port = port;
+ }
+
+ /**
+ * Get the x position of the HID.
+ *
+ * @return the x position of the HID
+ */
+ public final double getX() {
+ return getX(Hand.kRight);
+ }
+
+ /**
+ * Get the x position of HID.
+ *
+ * @param hand which hand, left or right
+ * @return the x position
+ */
+ public abstract double getX(Hand hand);
+
+ /**
+ * Get the y position of the HID.
+ *
+ * @return the y position
+ */
+ public final double getY() {
+ return getY(Hand.kRight);
+ }
+
+ /**
+ * Get the y position of the HID.
+ *
+ * @param hand which hand, left or right
+ * @return the y position
+ */
+ public abstract double getY(Hand hand);
+
+ /**
+ * Get the button value (starting at button 1).
+ *
+ * <p>The buttons are returned in a single 16 bit value with one bit representing the state of
+ * each button. The appropriate button is returned as a boolean value.
+ *
+ * @param button The button number to be read (starting at 1)
+ * @return The state of the button.
+ */
+ public boolean getRawButton(int button) {
+ return m_ds.getStickButton(m_port, (byte) button);
+ }
+
+ /**
+ * Whether the button was pressed since the last check. Button indexes begin at
+ * 1.
+ *
+ * @param button The button index, beginning at 1.
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getRawButtonPressed(int button) {
+ return m_ds.getStickButtonPressed(m_port, (byte) button);
+ }
+
+ /**
+ * Whether the button was released since the last check. Button indexes begin at
+ * 1.
+ *
+ * @param button The button index, beginning at 1.
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getRawButtonReleased(int button) {
+ return m_ds.getStickButtonReleased(m_port, button);
+ }
+
+ /**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+ public double getRawAxis(int axis) {
+ return m_ds.getStickAxis(m_port, axis);
+ }
+
+ /**
+ * Get the angle in degrees of a POV on the HID.
+ *
+ * <p>The POV angles start at 0 in the up direction, and increase clockwise (eg right is 90,
+ * upper-left is 315).
+ *
+ * @param pov The index of the POV to read (starting at 0)
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+ public int getPOV(int pov) {
+ return m_ds.getStickPOV(m_port, pov);
+ }
+
+ public int getPOV() {
+ return getPOV(0);
+ }
+
+ /**
+ * Get the number of axes for the HID.
+ *
+ * @return the number of axis for the current HID
+ */
+ public int getAxisCount() {
+ return m_ds.getStickAxisCount(m_port);
+ }
+
+ /**
+ * For the current HID, return the number of POVs.
+ */
+ public int getPOVCount() {
+ return m_ds.getStickPOVCount(m_port);
+ }
+
+ /**
+ * For the current HID, return the number of buttons.
+ */
+ public int getButtonCount() {
+ return m_ds.getStickButtonCount(m_port);
+ }
+
+ /**
+ * Get the type of the HID.
+ *
+ * @return the type of the HID.
+ */
+ public HIDType getType() {
+ return HIDType.of(m_ds.getJoystickType(m_port));
+ }
+
+ /**
+ * Get the name of the HID.
+ *
+ * @return the name of the HID.
+ */
+ public String getName() {
+ return m_ds.getJoystickName(m_port);
+ }
+
+ /**
+ * Get the axis type of a joystick axis.
+ *
+ * @return the axis type of a joystick axis.
+ */
+ public int getAxisType(int axis) {
+ return m_ds.getJoystickAxisType(m_port, axis);
+ }
+
+ /**
+ * Get the port number of the HID.
+ *
+ * @return The port number of the HID.
+ */
+ public int getPort() {
+ return m_port;
+ }
+
+ /**
+ * Set a single HID output value for the HID.
+ *
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value The value to set the output to
+ */
+ public void setOutput(int outputNumber, boolean value) {
+ m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
+ HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+ }
+
+ /**
+ * Set all HID output values for the HID.
+ *
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+ public void setOutputs(int value) {
+ m_outputs = value;
+ HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+ }
+
+ /**
+ * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and
+ * right rumble.
+ *
+ * @param type Which rumble value to set
+ * @param value The normalized value (0 to 1) to set the rumble to
+ */
+ public void setRumble(RumbleType type, double value) {
+ if (value < 0) {
+ value = 0;
+ } else if (value > 1) {
+ value = 1;
+ }
+ if (type == RumbleType.kLeftRumble) {
+ m_leftRumble = (short) (value * 65535);
+ } else {
+ m_rightRumble = (short) (value * 65535);
+ }
+ HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
new file mode 100644
index 0000000..4bd47e8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * GyroBase is the common base class for Gyro implementations such as AnalogGyro.
+ */
+public abstract class GyroBase extends SendableBase implements Gyro, PIDSource {
+ private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Set which parameter of the gyro you are using as a process control variable. The Gyro class
+ * supports the rate and displacement parameters
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on
+ * the set PIDSourceType
+ *
+ * @return the output according to the gyro
+ */
+ @Override
+ public double pidGet() {
+ switch (m_pidSource) {
+ case kRate:
+ return getRate();
+ case kDisplacement:
+ return getAngle();
+ default:
+ return 0.0;
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Gyro");
+ builder.addDoubleProperty("Value", this::getAngle, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
new file mode 100644
index 0000000..67a171c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
@@ -0,0 +1,377 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.I2CJNI;
+import edu.wpi.first.hal.util.BoundaryException;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * I2C bus interface class.
+ *
+ * <p>This class is intended to be used by sensor (and other I2C device) drivers. It probably should
+ * not be used directly.
+ */
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+public class I2C implements AutoCloseable {
+ public enum Port {
+ kOnboard(0), kMXP(1);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Port(int value) {
+ this.value = value;
+ }
+ }
+
+ private final int m_port;
+ private final int m_deviceAddress;
+
+ /**
+ * Constructor.
+ *
+ * @param port The I2C port the device is connected to.
+ * @param deviceAddress The address of the device on the I2C bus.
+ */
+ public I2C(Port port, int deviceAddress) {
+ m_port = port.value;
+ m_deviceAddress = deviceAddress;
+
+ I2CJNI.i2CInitialize((byte) port.value);
+
+ HAL.report(tResourceType.kResourceType_I2C, deviceAddress);
+ }
+
+ @Deprecated
+ public void free() {
+ close();
+ }
+
+ @Override
+ public void close() {
+ I2CJNI.i2CClose(m_port);
+ }
+
+ /**
+ * Generic transaction.
+ *
+ * <p>This is a lower-level interface to the I2C hardware giving you more control over each
+ * transaction.
+ *
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param sendSize Number of bytes to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param receiveSize Number of bytes to read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean transaction(byte[] dataToSend, int sendSize,
+ byte[] dataReceived, int receiveSize) {
+ if (dataToSend.length < sendSize) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
+ }
+ if (dataReceived.length < receiveSize) {
+ throw new IllegalArgumentException(
+ "dataReceived is too small, must be at least " + receiveSize);
+ }
+ return I2CJNI.i2CTransactionB(m_port, (byte) m_deviceAddress, dataToSend,
+ (byte) sendSize, dataReceived, (byte) receiveSize) < 0;
+ }
+
+ /**
+ * Generic transaction.
+ *
+ * <p>This is a lower-level interface to the I2C hardware giving you more control over each
+ * transaction.
+ *
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param sendSize Number of bytes to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param receiveSize Number of bytes to read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "ByteBufferBackingArray"})
+ public synchronized boolean transaction(ByteBuffer dataToSend, int sendSize,
+ ByteBuffer dataReceived, int receiveSize) {
+ if (dataToSend.hasArray() && dataReceived.hasArray()) {
+ return transaction(dataToSend.array(), sendSize, dataReceived.array(), receiveSize);
+ }
+ if (!dataToSend.isDirect()) {
+ throw new IllegalArgumentException("dataToSend must be a direct buffer");
+ }
+ if (dataToSend.capacity() < sendSize) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
+ }
+ if (!dataReceived.isDirect()) {
+ throw new IllegalArgumentException("dataReceived must be a direct buffer");
+ }
+ if (dataReceived.capacity() < receiveSize) {
+ throw new IllegalArgumentException(
+ "dataReceived is too small, must be at least " + receiveSize);
+ }
+
+ return I2CJNI.i2CTransaction(m_port, (byte) m_deviceAddress, dataToSend,
+ (byte) sendSize, dataReceived, (byte) receiveSize) < 0;
+ }
+
+ /**
+ * Attempt to address a device on the I2C bus.
+ *
+ * <p>This allows you to figure out if there is a device on the I2C bus that responds to the
+ * address specified in the constructor.
+ *
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public boolean addressOnly() {
+ return transaction(new byte[0], (byte) 0, new byte[0], (byte) 0);
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write a single byte to a register on a device and wait until the transaction is complete.
+ *
+ * @param registerAddress The address of the register on the device to be written.
+ * @param data The byte to write to the register on the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean write(int registerAddress, int data) {
+ byte[] buffer = new byte[2];
+ buffer[0] = (byte) registerAddress;
+ buffer[1] = (byte) data;
+ return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, buffer,
+ (byte) buffer.length) < 0;
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+ *
+ * @param data The data to write to the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean writeBulk(byte[] data) {
+ return writeBulk(data, data.length);
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+ *
+ * @param data The data to write to the device.
+ * @param size The number of data bytes to write.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public synchronized boolean writeBulk(byte[] data, int size) {
+ if (data.length < size) {
+ throw new IllegalArgumentException(
+ "buffer is too small, must be at least " + size);
+ }
+ return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
+ }
+
+ /**
+ * Execute a write transaction with the device.
+ *
+ * <p>Write multiple bytes to a register on a device and wait until the transaction is complete.
+ *
+ * @param data The data to write to the device.
+ * @param size The number of data bytes to write.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public synchronized boolean writeBulk(ByteBuffer data, int size) {
+ if (data.hasArray()) {
+ return writeBulk(data.array(), size);
+ }
+ if (!data.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (data.capacity() < size) {
+ throw new IllegalArgumentException(
+ "buffer is too small, must be at least " + size);
+ }
+
+ return I2CJNI.i2CWrite(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
+ }
+
+ /**
+ * Execute a read transaction with the device.
+ *
+ * <p>Read bytes from a device. Most I2C devices will auto-increment the register pointer
+ * internally allowing you to read consecutive registers on a device in a single transaction.
+ *
+ * @param registerAddress The register to read first in the transaction.
+ * @param count The number of bytes to read in the transaction.
+ * @param buffer A pointer to the array of bytes to store the data read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public boolean read(int registerAddress, int count, byte[] buffer) {
+ requireNonNull(buffer, "Null return buffer was given");
+
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count + " given");
+ }
+ if (buffer.length < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ byte[] registerAddressArray = new byte[1];
+ registerAddressArray[0] = (byte) registerAddress;
+
+ return transaction(registerAddressArray, registerAddressArray.length, buffer, count);
+ }
+
+ private ByteBuffer m_readDataToSendBuffer;
+
+ /**
+ * Execute a read transaction with the device.
+ *
+ * <p>Read bytes from a device. Most I2C devices will auto-increment the register pointer
+ * internally allowing you to read consecutive registers on a device in a single transaction.
+ *
+ * @param registerAddress The register to read first in the transaction.
+ * @param count The number of bytes to read in the transaction.
+ * @param buffer A buffer to store the data read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public boolean read(int registerAddress, int count, ByteBuffer buffer) {
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count + " given");
+ }
+
+ if (buffer.hasArray()) {
+ return read(registerAddress, count, buffer.array());
+ }
+
+ if (!buffer.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (buffer.capacity() < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ synchronized (this) {
+ if (m_readDataToSendBuffer == null) {
+ m_readDataToSendBuffer = ByteBuffer.allocateDirect(1);
+ }
+ m_readDataToSendBuffer.put(0, (byte) registerAddress);
+
+ return transaction(m_readDataToSendBuffer, 1, buffer, count);
+ }
+ }
+
+ /**
+ * Execute a read only transaction with the device.
+ *
+ * <p>Read bytes from a device. This method does not write any data to prompt the device.
+ *
+ * @param buffer A pointer to the array of bytes to store the data read from the device.
+ * @param count The number of bytes to read in the transaction.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ public boolean readOnly(byte[] buffer, int count) {
+ requireNonNull(buffer, "Null return buffer was given");
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count + " given");
+ }
+ if (buffer.length < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ return I2CJNI.i2CReadB(m_port, (byte) m_deviceAddress, buffer,
+ (byte) count) < 0;
+ }
+
+ /**
+ * Execute a read only transaction with the device.
+ *
+ * <p>Read bytes from a device. This method does not write any data to prompt the device.
+ *
+ * @param buffer A pointer to the array of bytes to store the data read from the device.
+ * @param count The number of bytes to read in the transaction.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public boolean readOnly(ByteBuffer buffer, int count) {
+ if (count < 1) {
+ throw new BoundaryException("Value must be at least 1, " + count
+ + " given");
+ }
+
+ if (buffer.hasArray()) {
+ return readOnly(buffer.array(), count);
+ }
+
+ if (!buffer.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (buffer.capacity() < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+
+ return I2CJNI.i2CRead(m_port, (byte) m_deviceAddress, buffer, (byte) count)
+ < 0;
+ }
+
+ /*
+ * Send a broadcast write to all devices on the I2C bus.
+ *
+ * <p>This is not currently implemented!
+ *
+ * @param registerAddress The register to write on all devices on the bus.
+ * @param data The value to write to the devices.
+ */
+ // public void broadcast(int registerAddress, int data) {
+ // }
+
+ /**
+ * Verify that a device's registers contain expected values.
+ *
+ * <p>Most devices will have a set of registers that contain a known value that can be used to
+ * identify them. This allows an I2C device driver to easily verify that the device contains the
+ * expected value.
+ *
+ * @param registerAddress The base register to start reading from the device.
+ * @param count The size of the field to be verified.
+ * @param expected A buffer containing the values expected from the device.
+ * @return true if the sensor was verified to be connected
+ * @pre The device must support and be configured to use register auto-increment.
+ */
+ public boolean verifySensor(int registerAddress, int count,
+ byte[] expected) {
+ // TODO: Make use of all 7 read bytes
+ byte[] dataToSend = new byte[1];
+
+ byte[] deviceData = new byte[4];
+ for (int i = 0; i < count; i += 4) {
+ int toRead = count - i < 4 ? count - i : 4;
+ // Read the chunk of data. Return false if the sensor does not
+ // respond.
+ dataToSend[0] = (byte) (registerAddress + i);
+ if (transaction(dataToSend, 1, deviceData, toRead)) {
+ return false;
+ }
+
+ for (byte j = 0; j < toRead; j++) {
+ if (deviceData[j] != expected[i + j]) {
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java
new file mode 100644
index 0000000..6fd807b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.InterruptJNI.InterruptJNIHandlerFunction;
+
+
+/**
+ * It is recommended that you use this class in conjunction with classes from {@link
+ * java.util.concurrent.atomic} as these objects are all thread safe.
+ *
+ * @param <T> The type of the parameter that should be returned to the the method {@link
+ * #interruptFired(int, Object)}
+ */
+public abstract class InterruptHandlerFunction<T> {
+ /**
+ * The entry point for the interrupt. When the interrupt fires the {@link #apply(int, Object)}
+ * method is called. The outer class is provided as an interface to allow the implementer to pass
+ * a generic object to the interrupt fired method.
+ */
+ private class Function implements InterruptJNIHandlerFunction {
+ @SuppressWarnings("unchecked")
+ @Override
+ public void apply(int interruptAssertedMask, Object param) {
+ interruptFired(interruptAssertedMask, (T) param);
+ }
+ }
+
+ final Function m_function = new Function();
+
+ /**
+ * This method is run every time an interrupt is fired.
+ *
+ * @param interruptAssertedMask Interrupt Mask
+ * @param param The parameter provided by overriding the {@link
+ * #overridableParameter()} method.
+ */
+ public abstract void interruptFired(int interruptAssertedMask, T param);
+
+
+ /**
+ * Override this method if you would like to pass a specific parameter to the {@link
+ * #interruptFired(int, Object)} when it is fired by the interrupt. This method is called once
+ * when {@link InterruptableSensorBase#requestInterrupts(InterruptHandlerFunction)} is run.
+ *
+ * @return The object that should be passed to the interrupt when it runs
+ */
+ public T overridableParameter() {
+ return null;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
new file mode 100644
index 0000000..cc10fe1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
@@ -0,0 +1,249 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.InterruptJNI;
+import edu.wpi.first.hal.util.AllocationException;
+
+
+/**
+ * Base for sensors to be used with interrupts.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class InterruptableSensorBase extends SendableBase {
+ @SuppressWarnings("JavadocMethod")
+ public enum WaitResult {
+ kTimeout(0x0), kRisingEdge(0x1), kFallingEdge(0x100), kBoth(0x101);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ WaitResult(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * The interrupt resource.
+ */
+ protected int m_interrupt = InterruptJNI.HalInvalidHandle;
+
+ /**
+ * Flags if the interrupt being allocated is synchronous.
+ */
+ protected boolean m_isSynchronousInterrupt;
+
+ /**
+ * Create a new InterrupatableSensorBase.
+ */
+ public InterruptableSensorBase() {
+ m_interrupt = 0;
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ if (m_interrupt != 0) {
+ cancelInterrupts();
+ }
+ }
+
+ /**
+ * If this is an analog trigger.
+ *
+ * @return true if this is an analog trigger.
+ */
+ public abstract int getAnalogTriggerTypeForRouting();
+
+ /**
+ * The channel routing number.
+ *
+ * @return channel routing number
+ */
+ public abstract int getPortHandleForRouting();
+
+ /**
+ * Request one of the 8 interrupts asynchronously on this digital input.
+ *
+ * @param handler The {@link InterruptHandlerFunction} that contains the method {@link
+ * InterruptHandlerFunction#interruptFired(int, Object)} that will be called
+ * whenever there is an interrupt on this device. Request interrupts in synchronous
+ * mode where the user program interrupt handler will be called when an interrupt
+ * occurs. The default is interrupt on rising edges only.
+ */
+ public void requestInterrupts(InterruptHandlerFunction<?> handler) {
+ if (m_interrupt != 0) {
+ throw new AllocationException("The interrupt has already been allocated");
+ }
+
+ allocateInterrupts(false);
+
+ assert m_interrupt != 0;
+
+ InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
+ getAnalogTriggerTypeForRouting());
+ setUpSourceEdge(true, false);
+ InterruptJNI.attachInterruptHandler(m_interrupt, handler.m_function,
+ handler.overridableParameter());
+ }
+
+ /**
+ * Request one of the 8 interrupts synchronously on this digital input. Request interrupts in
+ * synchronous mode where the user program will have to explicitly wait for the interrupt to occur
+ * using {@link #waitForInterrupt}. The default is interrupt on rising edges only.
+ */
+ public void requestInterrupts() {
+ if (m_interrupt != 0) {
+ throw new AllocationException("The interrupt has already been allocated");
+ }
+
+ allocateInterrupts(true);
+
+ assert m_interrupt != 0;
+
+ InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
+ getAnalogTriggerTypeForRouting());
+ setUpSourceEdge(true, false);
+
+ }
+
+ /**
+ * Allocate the interrupt.
+ *
+ * @param watcher true if the interrupt should be in synchronous mode where the user program will
+ * have to explicitly wait for the interrupt to occur.
+ */
+ protected void allocateInterrupts(boolean watcher) {
+ m_isSynchronousInterrupt = watcher;
+
+ m_interrupt = InterruptJNI.initializeInterrupts(watcher);
+ }
+
+ /**
+ * Cancel interrupts on this device. This deallocates all the chipobject structures and disables
+ * any interrupts.
+ */
+ public void cancelInterrupts() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ InterruptJNI.cleanInterrupts(m_interrupt);
+ m_interrupt = 0;
+ }
+
+ /**
+ * In synchronous mode, wait for the defined interrupt to occur.
+ *
+ * @param timeout Timeout in seconds
+ * @param ignorePrevious If true, ignore interrupts that happened before waitForInterrupt was
+ * called.
+ * @return Result of the wait.
+ */
+ public WaitResult waitForInterrupt(double timeout, boolean ignorePrevious) {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ int result = InterruptJNI.waitForInterrupt(m_interrupt, timeout, ignorePrevious);
+
+ // Rising edge result is the interrupt bit set in the byte 0xFF
+ // Falling edge result is the interrupt bit set in the byte 0xFF00
+ // Set any bit set to be true for that edge, and AND the 2 results
+ // together to match the existing enum for all interrupts
+ int rising = ((result & 0xFF) != 0) ? 0x1 : 0x0;
+ int falling = ((result & 0xFF00) != 0) ? 0x0100 : 0x0;
+ result = rising | falling;
+
+ for (WaitResult mode : WaitResult.values()) {
+ if (mode.value == result) {
+ return mode;
+ }
+ }
+ return null;
+ }
+
+ /**
+ * In synchronous mode, wait for the defined interrupt to occur.
+ *
+ * @param timeout Timeout in seconds
+ * @return Result of the wait.
+ */
+ public WaitResult waitForInterrupt(double timeout) {
+ return waitForInterrupt(timeout, true);
+ }
+
+ /**
+ * Enable interrupts to occur on this input. Interrupts are disabled when the RequestInterrupt
+ * call is made. This gives time to do the setup of the other options before starting to field
+ * interrupts.
+ */
+ public void enableInterrupts() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ if (m_isSynchronousInterrupt) {
+ throw new IllegalStateException("You do not need to enable synchronous interrupts");
+ }
+ InterruptJNI.enableInterrupts(m_interrupt);
+ }
+
+ /**
+ * Disable Interrupts without without deallocating structures.
+ */
+ public void disableInterrupts() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ if (m_isSynchronousInterrupt) {
+ throw new IllegalStateException("You can not disable synchronous interrupts");
+ }
+ InterruptJNI.disableInterrupts(m_interrupt);
+ }
+
+ /**
+ * Return the timestamp for the rising interrupt that occurred most recently. This is in the same
+ * time domain as getClock(). The rising-edge interrupt should be enabled with {@link
+ * #setUpSourceEdge}.
+ *
+ * @return Timestamp in seconds since boot.
+ */
+ public double readRisingTimestamp() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ return InterruptJNI.readInterruptRisingTimestamp(m_interrupt) * 1e-6;
+ }
+
+ /**
+ * Return the timestamp for the falling interrupt that occurred most recently. This is in the same
+ * time domain as getClock(). The falling-edge interrupt should be enabled with {@link
+ * #setUpSourceEdge}.
+ *
+ * @return Timestamp in seconds since boot.
+ */
+ public double readFallingTimestamp() {
+ if (m_interrupt == 0) {
+ throw new IllegalStateException("The interrupt is not allocated.");
+ }
+ return InterruptJNI.readInterruptFallingTimestamp(m_interrupt) * 1e-6;
+ }
+
+ /**
+ * Set which edge to trigger interrupts on.
+ *
+ * @param risingEdge true to interrupt on rising edge
+ * @param fallingEdge true to interrupt on falling edge
+ */
+ public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
+ if (m_interrupt != 0) {
+ InterruptJNI.setInterruptUpSourceEdge(m_interrupt, risingEdge,
+ fallingEdge);
+ } else {
+ throw new IllegalArgumentException("You must call RequestInterrupts before setUpSourceEdge");
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
new file mode 100644
index 0000000..e72033e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * IterativeRobot implements the IterativeRobotBase robot program framework.
+ *
+ * <p>The IterativeRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>periodic() functions from the base class are called each time a new packet is received from
+ * the driver station.
+ *
+ * @deprecated Use TimedRobot instead. It's a drop-in replacement that provides more regular
+ * execution periods.
+ */
+@Deprecated
+public class IterativeRobot extends IterativeRobotBase {
+ private static final double kPacketPeriod = 0.02;
+
+ /**
+ * Create a new IterativeRobot.
+ */
+ public IterativeRobot() {
+ super(kPacketPeriod);
+
+ HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ @Override
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ // Loop forever, calling the appropriate mode-dependent function
+ while (true) {
+ // Wait for new data to arrive
+ m_ds.waitForData();
+
+ loopFunc();
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
new file mode 100644
index 0000000..ee13ef9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
@@ -0,0 +1,275 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
+ * class.
+ *
+ * <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
+ * by teams directly.
+ *
+ * <p>This class provides the following functions which are called by the main loop,
+ * startCompetition(), at the appropriate times:
+ *
+ * <p>robotInit() -- provide for initialization at robot power-on
+ *
+ * <p>init() functions -- each of the following functions is called once when the
+ * appropriate mode is entered:
+ * - disabledInit() -- called each and every time disabled is entered from
+ * another mode
+ * - autonomousInit() -- called each and every time autonomous is entered from
+ * another mode
+ * - teleopInit() -- called each and every time teleop is entered from
+ * another mode
+ * - testInit() -- called each and every time test is entered from
+ * another mode
+ *
+ * <p>periodic() functions -- each of these functions is called on an interval:
+ * - robotPeriodic()
+ * - disabledPeriodic()
+ * - autonomousPeriodic()
+ * - teleopPeriodic()
+ * - testPeriodic()
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class IterativeRobotBase extends RobotBase {
+ protected double m_period;
+
+ private enum Mode {
+ kNone,
+ kDisabled,
+ kAutonomous,
+ kTeleop,
+ kTest
+ }
+
+ private Mode m_lastMode = Mode.kNone;
+ private final Watchdog m_watchdog;
+
+ /**
+ * Constructor for IterativeRobotBase.
+ *
+ * @param period Period in seconds.
+ */
+ protected IterativeRobotBase(double period) {
+ m_period = period;
+ m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ @Override
+ public abstract void startCompetition();
+
+ /* ----------- Overridable initialization code ----------------- */
+
+ /**
+ * Robot-wide initialization code should go here.
+ *
+ * <p>Users should override this method for default Robot-wide initialization which will be called
+ * when the robot is first powered on. It will be called exactly one time.
+ *
+ * <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
+ * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
+ * never indicate that the code is ready, causing the robot to be bypassed in a match.
+ */
+ public void robotInit() {
+ System.out.println("Default robotInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for disabled mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters disabled mode.
+ */
+ public void disabledInit() {
+ System.out.println("Default disabledInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for autonomous mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters autonomous mode.
+ */
+ public void autonomousInit() {
+ System.out.println("Default autonomousInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for teleop mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters teleop mode.
+ */
+ public void teleopInit() {
+ System.out.println("Default teleopInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for test mode should go here.
+ *
+ * <p>Users should override this method for initialization code which will be called each time the
+ * robot enters test mode.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void testInit() {
+ System.out.println("Default testInit() method... Overload me!");
+ }
+
+ /* ----------- Overridable periodic code ----------------- */
+
+ private boolean m_rpFirstRun = true;
+
+ /**
+ * Periodic code for all robot modes should go here.
+ */
+ public void robotPeriodic() {
+ if (m_rpFirstRun) {
+ System.out.println("Default robotPeriodic() method... Overload me!");
+ m_rpFirstRun = false;
+ }
+ }
+
+ private boolean m_dpFirstRun = true;
+
+ /**
+ * Periodic code for disabled mode should go here.
+ */
+ public void disabledPeriodic() {
+ if (m_dpFirstRun) {
+ System.out.println("Default disabledPeriodic() method... Overload me!");
+ m_dpFirstRun = false;
+ }
+ }
+
+ private boolean m_apFirstRun = true;
+
+ /**
+ * Periodic code for autonomous mode should go here.
+ */
+ public void autonomousPeriodic() {
+ if (m_apFirstRun) {
+ System.out.println("Default autonomousPeriodic() method... Overload me!");
+ m_apFirstRun = false;
+ }
+ }
+
+ private boolean m_tpFirstRun = true;
+
+ /**
+ * Periodic code for teleop mode should go here.
+ */
+ public void teleopPeriodic() {
+ if (m_tpFirstRun) {
+ System.out.println("Default teleopPeriodic() method... Overload me!");
+ m_tpFirstRun = false;
+ }
+ }
+
+ private boolean m_tmpFirstRun = true;
+
+ /**
+ * Periodic code for test mode should go here.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void testPeriodic() {
+ if (m_tmpFirstRun) {
+ System.out.println("Default testPeriodic() method... Overload me!");
+ m_tmpFirstRun = false;
+ }
+ }
+
+ protected void loopFunc() {
+ m_watchdog.reset();
+
+ // Call the appropriate function depending upon the current robot mode
+ if (isDisabled()) {
+ // Call DisabledInit() if we are now just entering disabled mode from either a different mode
+ // or from power-on.
+ if (m_lastMode != Mode.kDisabled) {
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ disabledInit();
+ m_watchdog.addEpoch("disabledInit()");
+ m_lastMode = Mode.kDisabled;
+ }
+
+ HAL.observeUserProgramDisabled();
+ disabledPeriodic();
+ m_watchdog.addEpoch("disablePeriodic()");
+ } else if (isAutonomous()) {
+ // Call AutonomousInit() if we are now just entering autonomous mode from either a different
+ // mode or from power-on.
+ if (m_lastMode != Mode.kAutonomous) {
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ autonomousInit();
+ m_watchdog.addEpoch("autonomousInit()");
+ m_lastMode = Mode.kAutonomous;
+ }
+
+ HAL.observeUserProgramAutonomous();
+ autonomousPeriodic();
+ m_watchdog.addEpoch("autonomousPeriodic()");
+ } else if (isOperatorControl()) {
+ // Call TeleopInit() if we are now just entering teleop mode from either a different mode or
+ // from power-on.
+ if (m_lastMode != Mode.kTeleop) {
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ teleopInit();
+ m_watchdog.addEpoch("teleopInit()");
+ m_lastMode = Mode.kTeleop;
+ }
+
+ HAL.observeUserProgramTeleop();
+ teleopPeriodic();
+ m_watchdog.addEpoch("teleopPeriodic()");
+ } else {
+ // Call TestInit() if we are now just entering test mode from either a different mode or from
+ // power-on.
+ if (m_lastMode != Mode.kTest) {
+ LiveWindow.setEnabled(true);
+ Shuffleboard.enableActuatorWidgets();
+ testInit();
+ m_watchdog.addEpoch("testInit()");
+ m_lastMode = Mode.kTest;
+ }
+
+ HAL.observeUserProgramTest();
+ testPeriodic();
+ m_watchdog.addEpoch("testPeriodic()");
+ }
+
+ robotPeriodic();
+ m_watchdog.addEpoch("robotPeriodic()");
+ m_watchdog.disable();
+ SmartDashboard.updateValues();
+
+ LiveWindow.updateValues();
+ Shuffleboard.update();
+
+ // Warn on loop time overruns
+ if (m_watchdog.isExpired()) {
+ m_watchdog.printEpochs();
+ }
+ }
+
+ private void printLoopOverrunMessage() {
+ DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
new file mode 100644
index 0000000..934e70b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device.
+ */
+public class Jaguar extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the Jaguar is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Jaguar(final int channel) {
+ super(channel);
+
+ /*
+ * Input profile defined by Luminary Micro.
+ *
+ * Full reverse ranges from 0.671325ms to 0.6972211ms Proportional reverse
+ * ranges from 0.6972211ms to 1.4482078ms Neutral ranges from 1.4482078ms to
+ * 1.5517922ms Proportional forward ranges from 1.5517922ms to 2.3027789ms
+ * Full forward ranges from 2.3027789ms to 2.328675ms
+ */
+ setBounds(2.31, 1.55, 1.507, 1.454, .697);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_Jaguar, getChannel());
+ setName("Jaguar", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
new file mode 100644
index 0000000..bb7ef81
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -0,0 +1,404 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Handle input from standard Joysticks connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each joystick
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class Joystick extends GenericHID {
+ static final byte kDefaultXChannel = 0;
+ static final byte kDefaultYChannel = 1;
+ static final byte kDefaultZChannel = 2;
+ static final byte kDefaultTwistChannel = 2;
+ static final byte kDefaultThrottleChannel = 3;
+
+ @Deprecated
+ static final byte kDefaultXAxis = 0;
+ @Deprecated
+ static final byte kDefaultYAxis = 1;
+ @Deprecated
+ static final byte kDefaultZAxis = 2;
+ @Deprecated
+ static final byte kDefaultTwistAxis = 2;
+ @Deprecated
+ static final byte kDefaultThrottleAxis = 3;
+
+ /**
+ * Represents an analog axis on a joystick.
+ */
+ public enum AxisType {
+ kX(0), kY(1), kZ(2), kTwist(3), kThrottle(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ AxisType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents a digital button on a joystick.
+ */
+ public enum ButtonType {
+ kTrigger(1), kTop(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ ButtonType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents a digital button on a joystick.
+ */
+ private enum Button {
+ kTrigger(1), kTop(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Button(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents an analog axis on a joystick.
+ */
+ private enum Axis {
+ kX(0), kY(1), kZ(2), kTwist(3), kThrottle(4), kNumAxes(5);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Axis(int value) {
+ this.value = value;
+ }
+ }
+
+ private final byte[] m_axes = new byte[Axis.kNumAxes.value];
+
+ /**
+ * Construct an instance of a joystick. The joystick index is the USB port on the drivers
+ * station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into.
+ */
+ public Joystick(final int port) {
+ super(port);
+
+ m_axes[Axis.kX.value] = kDefaultXChannel;
+ m_axes[Axis.kY.value] = kDefaultYChannel;
+ m_axes[Axis.kZ.value] = kDefaultZChannel;
+ m_axes[Axis.kTwist.value] = kDefaultTwistChannel;
+ m_axes[Axis.kThrottle.value] = kDefaultThrottleChannel;
+
+ HAL.report(tResourceType.kResourceType_Joystick, port);
+ }
+
+ /**
+ * Set the channel associated with the X axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setXChannel(int channel) {
+ m_axes[Axis.kX.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the Y axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setYChannel(int channel) {
+ m_axes[Axis.kY.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the Z axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setZChannel(int channel) {
+ m_axes[Axis.kZ.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the throttle axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setThrottleChannel(int channel) {
+ m_axes[Axis.kThrottle.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with the twist axis.
+ *
+ * @param channel The channel to set the axis to.
+ */
+ public void setTwistChannel(int channel) {
+ m_axes[Axis.kTwist.value] = (byte) channel;
+ }
+
+ /**
+ * Set the channel associated with a specified axis.
+ *
+ * @deprecated Use the more specific axis channel setter functions.
+ * @param axis The axis to set the channel for.
+ * @param channel The channel to set the axis to.
+ */
+ @Deprecated
+ public void setAxisChannel(AxisType axis, int channel) {
+ m_axes[axis.value] = (byte) channel;
+ }
+
+ /**
+ * Get the channel currently associated with the X axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getXChannel() {
+ return m_axes[Axis.kX.value];
+ }
+
+ /**
+ * Get the channel currently associated with the Y axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getYChannel() {
+ return m_axes[Axis.kY.value];
+ }
+
+ /**
+ * Get the channel currently associated with the Z axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getZChannel() {
+ return m_axes[Axis.kZ.value];
+ }
+
+ /**
+ * Get the channel currently associated with the twist axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getTwistChannel() {
+ return m_axes[Axis.kTwist.value];
+ }
+
+ /**
+ * Get the channel currently associated with the throttle axis.
+ *
+ * @return The channel for the axis.
+ */
+ public int getThrottleChannel() {
+ return m_axes[Axis.kThrottle.value];
+ }
+
+ /**
+ * Get the channel currently associated with the specified axis.
+ *
+ * @deprecated Use the more specific axis channel getter functions.
+ * @param axis The axis to look up the channel for.
+ * @return The channel for the axis.
+ */
+ @Deprecated
+ public int getAxisChannel(AxisType axis) {
+ return m_axes[axis.value];
+ }
+
+ /**
+ * Get the X value of the joystick. This depends on the mapping of the joystick connected to the
+ * current port.
+ *
+ * @param hand Unused
+ * @return The X value of the joystick.
+ */
+ @Override
+ public final double getX(Hand hand) {
+ return getRawAxis(m_axes[Axis.kX.value]);
+ }
+
+ /**
+ * Get the Y value of the joystick. This depends on the mapping of the joystick connected to the
+ * current port.
+ *
+ * @param hand Unused
+ * @return The Y value of the joystick.
+ */
+ @Override
+ public final double getY(Hand hand) {
+ return getRawAxis(m_axes[Axis.kY.value]);
+ }
+
+ /**
+ * Get the z position of the HID.
+ *
+ * @return the z position
+ */
+ public double getZ() {
+ return getRawAxis(m_axes[Axis.kZ.value]);
+ }
+
+ /**
+ * Get the twist value of the current joystick. This depends on the mapping of the joystick
+ * connected to the current port.
+ *
+ * @return The Twist value of the joystick.
+ */
+ public double getTwist() {
+ return getRawAxis(m_axes[Axis.kTwist.value]);
+ }
+
+ /**
+ * Get the throttle value of the current joystick. This depends on the mapping of the joystick
+ * connected to the current port.
+ *
+ * @return The Throttle value of the joystick.
+ */
+ public double getThrottle() {
+ return getRawAxis(m_axes[Axis.kThrottle.value]);
+ }
+
+ /**
+ * For the current joystick, return the axis determined by the argument.
+ *
+ * <p>This is for cases where the joystick axis is returned programmatically, otherwise one of the
+ * previous functions would be preferable (for example getX()).
+ *
+ * @deprecated Use the more specific axis getter functions.
+ * @param axis The axis to read.
+ * @return The value of the axis.
+ */
+ @Deprecated
+ public double getAxis(final AxisType axis) {
+ switch (axis) {
+ case kX:
+ return getX();
+ case kY:
+ return getY();
+ case kZ:
+ return getZ();
+ case kTwist:
+ return getTwist();
+ case kThrottle:
+ return getThrottle();
+ default:
+ return 0.0;
+ }
+ }
+
+ /**
+ * Read the state of the trigger on the joystick.
+ *
+ * @return The state of the trigger.
+ */
+ public boolean getTrigger() {
+ return getRawButton(Button.kTrigger.value);
+ }
+
+ /**
+ * Whether the trigger was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getTriggerPressed() {
+ return getRawButtonPressed(Button.kTrigger.value);
+ }
+
+ /**
+ * Whether the trigger was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getTriggerReleased() {
+ return getRawButtonReleased(Button.kTrigger.value);
+ }
+
+ /**
+ * Read the state of the top button on the joystick.
+ *
+ * @return The state of the top button.
+ */
+ public boolean getTop() {
+ return getRawButton(Button.kTop.value);
+ }
+
+ /**
+ * Whether the top button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getTopPressed() {
+ return getRawButtonPressed(Button.kTop.value);
+ }
+
+ /**
+ * Whether the top button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getTopReleased() {
+ return getRawButtonReleased(Button.kTop.value);
+ }
+
+ /**
+ * Get buttons based on an enumerated type.
+ *
+ * <p>The button type will be looked up in the list of buttons and then read.
+ *
+ * @deprecated Use Button enum values instead of ButtonType.
+ * @param button The type of button to read.
+ * @return The state of the button.
+ */
+ @Deprecated
+ public boolean getButton(ButtonType button) {
+ return getRawButton(button.value);
+ }
+
+ /**
+ * Get the magnitude of the direction vector formed by the joystick's current position relative to
+ * its origin.
+ *
+ * @return The magnitude of the direction vector
+ */
+ public double getMagnitude() {
+ return Math.sqrt(Math.pow(getX(), 2) + Math.pow(getY(), 2));
+ }
+
+ /**
+ * Get the direction of the vector formed by the joystick and its origin in radians.
+ *
+ * @return The direction of the vector in radians
+ */
+ public double getDirectionRadians() {
+ return Math.atan2(getX(), -getY());
+ }
+
+ /**
+ * Get the direction of the vector formed by the joystick and its origin in degrees.
+ *
+ * @return The direction of the vector in degrees
+ */
+ public double getDirectionDegrees() {
+ return Math.toDegrees(getDirectionRadians());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/JoystickBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/JoystickBase.java
new file mode 100644
index 0000000..56d4eba
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/JoystickBase.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * JoystickBase Interface.
+ *
+ * @deprecated Inherit directly from GenericHID instead.
+ */
+@Deprecated
+public abstract class JoystickBase extends GenericHID {
+ public JoystickBase(int port) {
+ super(port);
+ }
+
+ /**
+ * Get the z position of the HID.
+ *
+ * @param hand which hand, left or right
+ * @return the z position
+ */
+ public abstract double getZ(Hand hand);
+
+ public double getZ() {
+ return getZ(Hand.kRight);
+ }
+
+ /**
+ * Get the twist value.
+ *
+ * @return the twist value
+ */
+ public abstract double getTwist();
+
+ /**
+ * Get the throttle.
+ *
+ * @return the throttle value
+ */
+ public abstract double getThrottle();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
new file mode 100644
index 0000000..5dd3a4f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.LinkedHashSet;
+import java.util.Set;
+
+/**
+ * This base class runs a watchdog timer and calls the subclass's StopMotor()
+ * function if the timeout expires.
+ *
+ * <p>The subclass should call feed() whenever the motor value is updated.
+ */
+public abstract class MotorSafety {
+ private static final double kDefaultSafetyExpiration = 0.1;
+
+ private double m_expiration = kDefaultSafetyExpiration;
+ private boolean m_enabled;
+ private double m_stopTime = Timer.getFPGATimestamp();
+ private final Object m_thisMutex = new Object();
+ private static final Set<MotorSafety> m_instanceList = new LinkedHashSet<>();
+ private static final Object m_listMutex = new Object();
+
+ /**
+ * MotorSafety constructor.
+ */
+ public MotorSafety() {
+ synchronized (m_listMutex) {
+ m_instanceList.add(this);
+ }
+ }
+
+ /**
+ * Feed the motor safety object.
+ *
+ * <p>Resets the timer on this object that is used to do the timeouts.
+ */
+ public void feed() {
+ synchronized (m_thisMutex) {
+ m_stopTime = Timer.getFPGATimestamp() + m_expiration;
+ }
+ }
+
+ /**
+ * Set the expiration time for the corresponding motor safety object.
+ *
+ * @param expirationTime The timeout value in seconds.
+ */
+ public void setExpiration(double expirationTime) {
+ synchronized (m_thisMutex) {
+ m_expiration = expirationTime;
+ }
+ }
+
+ /**
+ * Retrieve the timeout value for the corresponding motor safety object.
+ *
+ * @return the timeout value in seconds.
+ */
+ public double getExpiration() {
+ synchronized (m_thisMutex) {
+ return m_expiration;
+ }
+ }
+
+ /**
+ * Determine of the motor is still operating or has timed out.
+ *
+ * @return a true value if the motor is still operating normally and hasn't timed out.
+ */
+ public boolean isAlive() {
+ synchronized (m_thisMutex) {
+ return !m_enabled || m_stopTime > Timer.getFPGATimestamp();
+ }
+ }
+
+ /**
+ * Check if this motor has exceeded its timeout. This method is called periodically to determine
+ * if this motor has exceeded its timeout value. If it has, the stop method is called, and the
+ * motor is shut down until its value is updated again.
+ */
+ public void check() {
+ boolean enabled;
+ double stopTime;
+
+ synchronized (m_thisMutex) {
+ enabled = m_enabled;
+ stopTime = m_stopTime;
+ }
+
+ if (!enabled || RobotState.isDisabled() || RobotState.isTest()) {
+ return;
+ }
+
+ if (stopTime < Timer.getFPGATimestamp()) {
+ DriverStation.reportError(getDescription() + "... Output not updated often enough.", false);
+
+ stopMotor();
+ }
+ }
+
+ /**
+ * Enable/disable motor safety for this device.
+ *
+ * <p>Turn on and off the motor safety option for this PWM object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+ public void setSafetyEnabled(boolean enabled) {
+ synchronized (m_thisMutex) {
+ m_enabled = enabled;
+ }
+ }
+
+ /**
+ * Return the state of the motor safety enabled flag.
+ *
+ * <p>Return if the motor safety is currently enabled for this device.
+ *
+ * @return True if motor safety is enforced for this device
+ */
+ public boolean isSafetyEnabled() {
+ synchronized (m_thisMutex) {
+ return m_enabled;
+ }
+ }
+
+ /**
+ * Check the motors to see if any have timed out.
+ *
+ * <p>This static method is called periodically to poll all the motors and stop any that have
+ * timed out.
+ */
+ public static void checkMotors() {
+ synchronized (m_listMutex) {
+ for (MotorSafety elem : m_instanceList) {
+ elem.check();
+ }
+ }
+ }
+
+ public abstract void stopMotor();
+
+ public abstract String getDescription();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java
new file mode 100644
index 0000000..bf972c9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * The interface for sendable objects that gives the sendable a default name in the Smart
+ * Dashboard.
+ * @deprecated Use Sendable directly instead
+ */
+@Deprecated
+public interface NamedSendable extends Sendable {
+ /**
+ * The name of the subtable.
+ *
+ * @return the name of the subtable of SmartDashboard that the Sendable object will use.
+ */
+ @Override
+ String getName();
+
+ @Override
+ default void setName(String name) {
+ }
+
+ @Override
+ default String getSubsystem() {
+ return "";
+ }
+
+ @Override
+ default void setSubsystem(String subsystem) {
+ }
+
+ @Override
+ default void initSendable(SendableBuilder builder) {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
new file mode 100644
index 0000000..f166b46
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
@@ -0,0 +1,210 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Nidec Brushless Motor.
+ */
+public class NidecBrushless extends MotorSafety implements SpeedController, Sendable,
+ AutoCloseable {
+ private boolean m_isInverted;
+ private final DigitalOutput m_dio;
+ private final PWM m_pwm;
+ private volatile double m_speed;
+ private volatile boolean m_disabled;
+
+ private final SendableImpl m_sendableImpl;
+
+ /**
+ * Constructor.
+ *
+ * @param pwmChannel The PWM channel that the Nidec Brushless controller is attached to.
+ * 0-9 are on-board, 10-19 are on the MXP port
+ * @param dioChannel The DIO channel that the Nidec Brushless controller is attached to.
+ * 0-9 are on-board, 10-25 are on the MXP port
+ */
+ public NidecBrushless(final int pwmChannel, final int dioChannel) {
+ m_sendableImpl = new SendableImpl(true);
+
+ setSafetyEnabled(false);
+
+ // the dio controls the output (in PWM mode)
+ m_dio = new DigitalOutput(dioChannel);
+ addChild(m_dio);
+ m_dio.setPWMRate(15625);
+ m_dio.enablePWM(0.5);
+
+ // the pwm enables the controller
+ m_pwm = new PWM(pwmChannel);
+ addChild(m_pwm);
+
+ HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel);
+ setName("Nidec Brushless", pwmChannel);
+ }
+
+ @Override
+ public void close() {
+ m_sendableImpl.close();
+ m_dio.close();
+ m_pwm.close();
+ }
+
+ @Override
+ public final synchronized String getName() {
+ return m_sendableImpl.getName();
+ }
+
+ @Override
+ public final synchronized void setName(String name) {
+ m_sendableImpl.setName(name);
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ */
+ protected final void setName(String moduleType, int channel) {
+ m_sendableImpl.setName(moduleType, channel);
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ */
+ protected final void setName(String moduleType, int moduleNumber, int channel) {
+ m_sendableImpl.setName(moduleType, moduleNumber, channel);
+ }
+
+ @Override
+ public final synchronized String getSubsystem() {
+ return m_sendableImpl.getSubsystem();
+ }
+
+ @Override
+ public final synchronized void setSubsystem(String subsystem) {
+ m_sendableImpl.setSubsystem(subsystem);
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ protected final void addChild(Object child) {
+ m_sendableImpl.addChild(child);
+ }
+
+ /**
+ * Set the PWM value.
+ *
+ * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+ * FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+ @Override
+ public void set(double speed) {
+ if (!m_disabled) {
+ m_speed = speed;
+ m_dio.updateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
+ m_pwm.setRaw(0xffff);
+ }
+
+ feed();
+ }
+
+ /**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+ @Override
+ public double get() {
+ return m_speed;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ /**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+
+ /**
+ * Stop the motor. This is called by the MotorSafety object when it has a timeout for this PWM and
+ * needs to stop it from running. Calling set() will re-enable the motor.
+ */
+ @Override
+ public void stopMotor() {
+ m_dio.updateDutyCycle(0.5);
+ m_pwm.setDisabled();
+ }
+
+ @Override
+ public String getDescription() {
+ return "Nidec " + getChannel();
+ }
+
+ /**
+ * Disable the motor. The enable() function must be called to re-enable
+ * the motor.
+ */
+ @Override
+ public void disable() {
+ m_disabled = true;
+ m_dio.updateDutyCycle(0.5);
+ m_pwm.setDisabled();
+ }
+
+ /**
+ * Re-enable the motor after disable() has been called. The set()
+ * function must be called to set a new motor speed.
+ */
+ public void enable() {
+ m_disabled = false;
+ }
+
+ /**
+ * Gets the channel number associated with the object.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_pwm.getChannel();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Nidec Brushless");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
new file mode 100644
index 0000000..45b7d94
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -0,0 +1,198 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.NotifierJNI;
+
+public class Notifier implements AutoCloseable {
+ // The thread waiting on the HAL alarm.
+ private Thread m_thread;
+ // The lock for the process information.
+ private final ReentrantLock m_processLock = new ReentrantLock();
+ // The C pointer to the notifier object. We don't use it directly, it is
+ // just passed to the JNI bindings.
+ private final AtomicInteger m_notifier = new AtomicInteger();
+ // The time, in microseconds, at which the corresponding handler should be
+ // called. Has the same zero as Utility.getFPGATime().
+ private double m_expirationTime;
+ // The handler passed in by the user which should be called at the
+ // appropriate interval.
+ private Runnable m_handler;
+ // Whether we are calling the handler just once or periodically.
+ private boolean m_periodic;
+ // If periodic, the period of the calling; if just once, stores how long it
+ // is until we call the handler.
+ private double m_period;
+
+ @Override
+ @SuppressWarnings("NoFinalizer")
+ protected void finalize() {
+ close();
+ }
+
+ @Override
+ public void close() {
+ int handle = m_notifier.getAndSet(0);
+ if (handle == 0) {
+ return;
+ }
+ NotifierJNI.stopNotifier(handle);
+ // Join the thread to ensure the handler has exited.
+ if (m_thread.isAlive()) {
+ try {
+ m_thread.interrupt();
+ m_thread.join();
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ }
+ NotifierJNI.cleanNotifier(handle);
+ m_thread = null;
+ }
+
+ /**
+ * Update the alarm hardware to reflect the next alarm.
+ *
+ * @param triggerTime the time at which the next alarm will be triggered
+ */
+ private void updateAlarm(long triggerTime) {
+ int notifier = m_notifier.get();
+ if (notifier == 0) {
+ return;
+ }
+ NotifierJNI.updateNotifierAlarm(notifier, triggerTime);
+ }
+
+ /**
+ * Update the alarm hardware to reflect the next alarm.
+ */
+ private void updateAlarm() {
+ updateAlarm((long) (m_expirationTime * 1e6));
+ }
+
+ /**
+ * Create a Notifier for timer event notification.
+ *
+ * @param run The handler that is called at the notification time which is set
+ * using StartSingle or StartPeriodic.
+ */
+ public Notifier(Runnable run) {
+ m_handler = run;
+ m_notifier.set(NotifierJNI.initializeNotifier());
+
+ m_thread = new Thread(() -> {
+ while (!Thread.interrupted()) {
+ int notifier = m_notifier.get();
+ if (notifier == 0) {
+ break;
+ }
+ long curTime = NotifierJNI.waitForNotifierAlarm(notifier);
+ if (curTime == 0) {
+ break;
+ }
+
+ Runnable handler = null;
+ m_processLock.lock();
+ try {
+ handler = m_handler;
+ if (m_periodic) {
+ m_expirationTime += m_period;
+ updateAlarm();
+ } else {
+ // need to update the alarm to cause it to wait again
+ updateAlarm((long) -1);
+ }
+ } finally {
+ m_processLock.unlock();
+ }
+
+ if (handler != null) {
+ handler.run();
+ }
+ }
+ });
+ m_thread.setName("Notifier");
+ m_thread.setDaemon(true);
+ m_thread.setUncaughtExceptionHandler((thread, error) -> {
+ Throwable cause = error.getCause();
+ if (cause != null) {
+ error = cause;
+ }
+ DriverStation.reportError("Unhandled exception: " + error.toString(), error.getStackTrace());
+ DriverStation.reportError(
+ "The loopFunc() method (or methods called by it) should have handled "
+ + "the exception above.", false);
+ });
+ m_thread.start();
+ }
+
+ /**
+ * Change the handler function.
+ *
+ * @param handler Handler
+ */
+ public void setHandler(Runnable handler) {
+ m_processLock.lock();
+ try {
+ m_handler = handler;
+ } finally {
+ m_processLock.unlock();
+ }
+ }
+
+ /**
+ * Register for single event notification. A timer event is queued for a single
+ * event after the specified delay.
+ *
+ * @param delay Seconds to wait before the handler is called.
+ */
+ public void startSingle(double delay) {
+ m_processLock.lock();
+ try {
+ m_periodic = false;
+ m_period = delay;
+ m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
+ updateAlarm();
+ } finally {
+ m_processLock.unlock();
+ }
+ }
+
+ /**
+ * Register for periodic event notification. A timer event is queued for
+ * periodic event notification. Each time the interrupt occurs, the event will
+ * be immediately requeued for the same time interval.
+ *
+ * @param period Period in seconds to call the handler starting one period after
+ * the call to this method.
+ */
+ public void startPeriodic(double period) {
+ m_processLock.lock();
+ try {
+ m_periodic = true;
+ m_period = period;
+ m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
+ updateAlarm();
+ } finally {
+ m_processLock.unlock();
+ }
+ }
+
+ /**
+ * Stop timer events from occurring. Stop any repeating timer events from
+ * occurring. This will also remove any single notification events from the
+ * queue. If a timer-based call to the registered handler is in progress, this
+ * function will block until the handler call is complete.
+ */
+ public void stop() {
+ NotifierJNI.cancelNotifierAlarm(m_notifier.get());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
new file mode 100644
index 0000000..780a5c2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
@@ -0,0 +1,828 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.BoundaryException;
+import edu.wpi.first.wpilibj.filters.LinearDigitalFilter;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
+ * calculations, as well as writing the given PIDOutput.
+ *
+ * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
+ * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
+ * given set of PID constants.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class PIDBase extends SendableBase implements PIDInterface, PIDOutput {
+ public static final double kDefaultPeriod = 0.05;
+ private static int instances;
+
+ // Factor for "proportional" control
+ @SuppressWarnings("MemberName")
+ private double m_P;
+
+ // Factor for "integral" control
+ @SuppressWarnings("MemberName")
+ private double m_I;
+
+ // Factor for "derivative" control
+ @SuppressWarnings("MemberName")
+ private double m_D;
+
+ // Factor for "feed forward" control
+ @SuppressWarnings("MemberName")
+ private double m_F;
+
+ // |maximum output|
+ private double m_maximumOutput = 1.0;
+
+ // |minimum output|
+ private double m_minimumOutput = -1.0;
+
+ // Maximum input - limit setpoint to this
+ private double m_maximumInput;
+
+ // Minimum input - limit setpoint to this
+ private double m_minimumInput;
+
+ // Input range - difference between maximum and minimum
+ private double m_inputRange;
+
+ // Do the endpoints wrap around? (e.g., absolute encoder)
+ private boolean m_continuous;
+
+ // Is the PID controller enabled
+ protected boolean m_enabled;
+
+ // The prior error (used to compute velocity)
+ private double m_prevError;
+
+ // The sum of the errors for use in the integral calc
+ private double m_totalError;
+
+ // The tolerance object used to check if on target
+ private Tolerance m_tolerance;
+
+ private double m_setpoint;
+ private double m_prevSetpoint;
+ @SuppressWarnings("PMD.UnusedPrivateField")
+ private double m_error;
+ private double m_result;
+
+ private PIDSource m_origSource;
+ private LinearDigitalFilter m_filter;
+
+ protected ReentrantLock m_thisMutex = new ReentrantLock();
+
+ // Ensures when disable() is called, pidWrite() won't run if calculate()
+ // is already running at that time.
+ protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
+
+ protected PIDSource m_pidInput;
+ protected PIDOutput m_pidOutput;
+ protected Timer m_setpointTimer;
+
+ /**
+ * Tolerance is the type of tolerance used to specify if the PID controller is on target.
+ *
+ * <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
+ * specify types of tolerance specifications to use.
+ */
+ public interface Tolerance {
+ boolean onTarget();
+ }
+
+ /**
+ * Used internally for when Tolerance hasn't been set.
+ */
+ public static class NullTolerance implements Tolerance {
+ @Override
+ public boolean onTarget() {
+ throw new IllegalStateException("No tolerance value set when calling onTarget().");
+ }
+ }
+
+ public class PercentageTolerance implements Tolerance {
+ private final double m_percentage;
+
+ PercentageTolerance(double value) {
+ m_percentage = value;
+ }
+
+ @Override
+ public boolean onTarget() {
+ return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
+ }
+ }
+
+ public class AbsoluteTolerance implements Tolerance {
+ private final double m_value;
+
+ AbsoluteTolerance(double value) {
+ m_value = value;
+ }
+
+ @Override
+ public boolean onTarget() {
+ return Math.abs(getError()) < m_value;
+ }
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, and F.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param Kf the feed forward term
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source,
+ PIDOutput output) {
+ super(false);
+ requireNonNull(source, "Null PIDSource was given");
+ requireNonNull(output, "Null PIDOutput was given");
+
+ m_setpointTimer = new Timer();
+ m_setpointTimer.start();
+
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ // Save original source
+ m_origSource = source;
+
+ // Create LinearDigitalFilter with original source as its source argument
+ m_filter = LinearDigitalFilter.movingAverage(m_origSource, 1);
+ m_pidInput = m_filter;
+
+ m_pidOutput = output;
+
+ instances++;
+ HAL.report(tResourceType.kResourceType_PIDController, instances);
+ m_tolerance = new NullTolerance();
+ setName("PIDController", instances);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, and D.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source the PIDSource object that is used to get values
+ * @param output the PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+ this(Kp, Ki, Kd, 0.0, source, output);
+ }
+
+ /**
+ * Read the input, calculate the output accordingly, and write to the output. This should only be
+ * called by the PIDTask and is created during initialization.
+ */
+ @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.NPathComplexity"})
+ protected void calculate() {
+ if (m_origSource == null || m_pidOutput == null) {
+ return;
+ }
+
+ boolean enabled;
+
+ m_thisMutex.lock();
+ try {
+ enabled = m_enabled;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ if (enabled) {
+ double input;
+
+ // Storage for function inputs
+ PIDSourceType pidSourceType;
+ double P;
+ double I;
+ double D;
+ double feedForward = calculateFeedForward();
+ double minimumOutput;
+ double maximumOutput;
+
+ // Storage for function input-outputs
+ double prevError;
+ double error;
+ double totalError;
+
+ m_thisMutex.lock();
+ try {
+ input = m_pidInput.pidGet();
+
+ pidSourceType = m_pidInput.getPIDSourceType();
+ P = m_P;
+ I = m_I;
+ D = m_D;
+ minimumOutput = m_minimumOutput;
+ maximumOutput = m_maximumOutput;
+
+ prevError = m_prevError;
+ error = getContinuousError(m_setpoint - input);
+ totalError = m_totalError;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ // Storage for function outputs
+ double result;
+
+ if (pidSourceType.equals(PIDSourceType.kRate)) {
+ if (P != 0) {
+ totalError = clamp(totalError + error, minimumOutput / P,
+ maximumOutput / P);
+ }
+
+ result = P * totalError + D * error + feedForward;
+ } else {
+ if (I != 0) {
+ totalError = clamp(totalError + error, minimumOutput / I,
+ maximumOutput / I);
+ }
+
+ result = P * error + I * totalError + D * (error - prevError)
+ + feedForward;
+ }
+
+ result = clamp(result, minimumOutput, maximumOutput);
+
+ // Ensures m_enabled check and pidWrite() call occur atomically
+ m_pidWriteMutex.lock();
+ try {
+ m_thisMutex.lock();
+ try {
+ if (m_enabled) {
+ // Don't block other PIDController operations on pidWrite()
+ m_thisMutex.unlock();
+
+ m_pidOutput.pidWrite(result);
+ }
+ } finally {
+ if (m_thisMutex.isHeldByCurrentThread()) {
+ m_thisMutex.unlock();
+ }
+ }
+ } finally {
+ m_pidWriteMutex.unlock();
+ }
+
+ m_thisMutex.lock();
+ try {
+ m_prevError = error;
+ m_error = error;
+ m_totalError = totalError;
+ m_result = result;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+ }
+
+ /**
+ * Calculate the feed forward term.
+ *
+ * <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
+ * feed forward calculation is desired, the user can override this function and provide his or
+ * her own. This function does no synchronization because the PIDController class only calls it
+ * in synchronized code, so be careful if calling it oneself.
+ *
+ * <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
+ * setpoint for the output. If a position PID controller is being used, the F term should be set
+ * to 1 over the maximum speed for the output measured in setpoint units per this controller's
+ * update period (see the default period in this class's constructor).
+ */
+ protected double calculateFeedForward() {
+ if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
+ return m_F * getSetpoint();
+ } else {
+ double temp = m_F * getDeltaSetpoint();
+ m_prevSetpoint = m_setpoint;
+ m_setpointTimer.reset();
+ return temp;
+ }
+ }
+
+ /**
+ * Set the PID Controller gain parameters. Set the proportional, integral, and differential
+ * coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ */
+ @Override
+ @SuppressWarnings("ParameterName")
+ public void setPID(double p, double i, double d) {
+ m_thisMutex.lock();
+ try {
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the PID Controller gain parameters. Set the proportional, integral, and differential
+ * coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ * @param f Feed forward coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setPID(double p, double i, double d, double f) {
+ m_thisMutex.lock();
+ try {
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ m_F = f;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Proportional coefficient of the PID controller gain.
+ *
+ * @param p Proportional coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setP(double p) {
+ m_thisMutex.lock();
+ try {
+ m_P = p;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Integral coefficient of the PID controller gain.
+ *
+ * @param i Integral coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setI(double i) {
+ m_thisMutex.lock();
+ try {
+ m_I = i;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Differential coefficient of the PID controller gain.
+ *
+ * @param d differential coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setD(double d) {
+ m_thisMutex.lock();
+ try {
+ m_D = d;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the Feed forward coefficient of the PID controller gain.
+ *
+ * @param f feed forward coefficient
+ */
+ @SuppressWarnings("ParameterName")
+ public void setF(double f) {
+ m_thisMutex.lock();
+ try {
+ m_F = f;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+ @Override
+ public double getP() {
+ m_thisMutex.lock();
+ try {
+ return m_P;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Integral coefficient.
+ *
+ * @return integral coefficient
+ */
+ @Override
+ public double getI() {
+ m_thisMutex.lock();
+ try {
+ return m_I;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Differential coefficient.
+ *
+ * @return differential coefficient
+ */
+ @Override
+ public double getD() {
+ m_thisMutex.lock();
+ try {
+ return m_D;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Get the Feed forward coefficient.
+ *
+ * @return feed forward coefficient
+ */
+ public double getF() {
+ m_thisMutex.lock();
+ try {
+ return m_F;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Return the current PID result This is always centered on zero and constrained the the max and
+ * min outs.
+ *
+ * @return the latest calculated output
+ */
+ public double get() {
+ m_thisMutex.lock();
+ try {
+ return m_result;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the PID controller to consider the input to be continuous, Rather then using the max and
+ * min input range as constraints, it considers them to be the same point and automatically
+ * calculates the shortest route to the setpoint.
+ *
+ * @param continuous Set to true turns on continuous, false turns off continuous
+ */
+ public void setContinuous(boolean continuous) {
+ if (continuous && m_inputRange <= 0) {
+ throw new IllegalStateException("No input range set when calling setContinuous().");
+ }
+ m_thisMutex.lock();
+ try {
+ m_continuous = continuous;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the PID controller to consider the input to be continuous, Rather then using the max and
+ * min input range as constraints, it considers them to be the same point and automatically
+ * calculates the shortest route to the setpoint.
+ */
+ public void setContinuous() {
+ setContinuous(true);
+ }
+
+ /**
+ * Sets the maximum and minimum values expected from the input and setpoint.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the input
+ */
+ public void setInputRange(double minimumInput, double maximumInput) {
+ m_thisMutex.lock();
+ try {
+ if (minimumInput > maximumInput) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ setSetpoint(m_setpoint);
+ }
+
+ /**
+ * Sets the minimum and maximum values to write.
+ *
+ * @param minimumOutput the minimum percentage to write to the output
+ * @param maximumOutput the maximum percentage to write to the output
+ */
+ public void setOutputRange(double minimumOutput, double maximumOutput) {
+ m_thisMutex.lock();
+ try {
+ if (minimumOutput > maximumOutput) {
+ throw new BoundaryException("Lower bound is greater than upper bound");
+ }
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the setpoint for the PIDController.
+ *
+ * @param setpoint the desired setpoint
+ */
+ @Override
+ public void setSetpoint(double setpoint) {
+ m_thisMutex.lock();
+ try {
+ if (m_maximumInput > m_minimumInput) {
+ if (setpoint > m_maximumInput) {
+ m_setpoint = m_maximumInput;
+ } else if (setpoint < m_minimumInput) {
+ m_setpoint = m_minimumInput;
+ } else {
+ m_setpoint = setpoint;
+ }
+ } else {
+ m_setpoint = setpoint;
+ }
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the current setpoint of the PIDController.
+ *
+ * @return the current setpoint
+ */
+ @Override
+ public double getSetpoint() {
+ m_thisMutex.lock();
+ try {
+ return m_setpoint;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the change in setpoint over time of the PIDController.
+ *
+ * @return the change in setpoint over time
+ */
+ public double getDeltaSetpoint() {
+ m_thisMutex.lock();
+ try {
+ return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the current difference of the input from the setpoint.
+ *
+ * @return the current error
+ */
+ @Override
+ public double getError() {
+ m_thisMutex.lock();
+ try {
+ return getContinuousError(getSetpoint() - m_pidInput.pidGet());
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the current difference of the error over the past few iterations. You can specify the
+ * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
+ * used for the onTarget() function.
+ *
+ * @deprecated Use getError(), which is now already filtered.
+ * @return the current average of the error
+ */
+ @Deprecated
+ public double getAvgError() {
+ m_thisMutex.lock();
+ try {
+ return getError();
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Sets what type of input the PID controller will use.
+ *
+ * @param pidSource the type of input
+ */
+ void setPIDSourceType(PIDSourceType pidSource) {
+ m_pidInput.setPIDSourceType(pidSource);
+ }
+
+ /**
+ * Returns the type of input the PID controller is using.
+ *
+ * @return the PID controller input type
+ */
+ PIDSourceType getPIDSourceType() {
+ return m_pidInput.getPIDSourceType();
+ }
+
+ /**
+ * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
+ * the range or as an absolute value. The Tolerance object encapsulates those options in an
+ * object. Use it by creating the type of tolerance that you want to use: setTolerance(new
+ * PIDController.AbsoluteTolerance(0.1))
+ *
+ * @deprecated Use setPercentTolerance() instead.
+ * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
+ * AbsoluteTolerance
+ */
+ @Deprecated
+ public void setTolerance(Tolerance tolerance) {
+ m_tolerance = tolerance;
+ }
+
+ /**
+ * Set the absolute error which is considered tolerable for use with OnTarget.
+ *
+ * @param absvalue absolute error which is tolerable in the units of the input object
+ */
+ public void setAbsoluteTolerance(double absvalue) {
+ m_thisMutex.lock();
+ try {
+ m_tolerance = new AbsoluteTolerance(absvalue);
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
+ * 15 percent)
+ *
+ * @param percentage percent error which is tolerable
+ */
+ public void setPercentTolerance(double percentage) {
+ m_thisMutex.lock();
+ try {
+ m_tolerance = new PercentageTolerance(percentage);
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the number of previous error samples to average for tolerancing. When determining whether a
+ * mechanism is on target, the user may want to use a rolling average of previous measurements
+ * instead of a precise position or velocity. This is useful for noisy sensors which return a few
+ * erroneous measurements when the mechanism is on target. However, the mechanism will not
+ * register as on target for at least the specified bufLength cycles.
+ *
+ * @deprecated Use a LinearDigitalFilter as the input.
+ * @param bufLength Number of previous cycles to average.
+ */
+ @Deprecated
+ public void setToleranceBuffer(int bufLength) {
+ m_thisMutex.lock();
+ try {
+ m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength);
+ m_pidInput = m_filter;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Return true if the error is within the percentage of the total input range, determined by
+ * setTolerance. This assumes that the maximum and minimum input were set using setInput.
+ *
+ * @return true if the error is less than the tolerance
+ */
+ public boolean onTarget() {
+ m_thisMutex.lock();
+ try {
+ return m_tolerance.onTarget();
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Reset the previous error, the integral term, and disable the controller.
+ */
+ @Override
+ public void reset() {
+ m_thisMutex.lock();
+ try {
+ m_prevError = 0;
+ m_totalError = 0;
+ m_result = 0;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Passes the output directly to setSetpoint().
+ *
+ * <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
+ * In that case, the output of the parent controller becomes the input (i.e., the reference) of
+ * the child.
+ *
+ * <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
+ */
+ @Override
+ public void pidWrite(double output) {
+ setSetpoint(output);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PIDController");
+ builder.setSafeState(this::reset);
+ builder.addDoubleProperty("p", this::getP, this::setP);
+ builder.addDoubleProperty("i", this::getI, this::setI);
+ builder.addDoubleProperty("d", this::getD, this::setD);
+ builder.addDoubleProperty("f", this::getF, this::setF);
+ builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+ }
+
+ /**
+ * Wraps error around for continuous inputs. The original error is returned if continuous mode is
+ * disabled. This is an unsynchronized function.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+ protected double getContinuousError(double error) {
+ if (m_continuous && m_inputRange > 0) {
+ error %= m_inputRange;
+ if (Math.abs(error) > m_inputRange / 2) {
+ if (error > 0) {
+ return error - m_inputRange;
+ } else {
+ return error + m_inputRange;
+ }
+ }
+ }
+
+ return error;
+ }
+
+ private static double clamp(double value, double low, double high) {
+ return Math.max(low, Math.min(value, high));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
new file mode 100644
index 0000000..da4118f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -0,0 +1,181 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
+ * calculations, as well as writing the given PIDOutput.
+ *
+ * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
+ * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
+ * given set of PID constants.
+ */
+public class PIDController extends PIDBase implements Controller {
+ Notifier m_controlLoop = new Notifier(this::calculate);
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, and F.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param Kf the feed forward term
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ * @param period the loop time for doing calculations in seconds.
+ * This particularly affects calculations of
+ * the integral and differential terms.
+ * The default is 0.05 (50ms).
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source,
+ PIDOutput output, double period) {
+ super(Kp, Ki, Kd, Kf, source, output);
+ m_controlLoop.startPeriodic(period);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D and period.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source the PIDSource object that is used to get values
+ * @param output the PIDOutput object that is set to the output percentage
+ * @param period the loop time for doing calculations in seconds.
+ * This particularly affects calculations of
+ * the integral and differential terms.
+ * The default is 0.05 (50ms).
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output,
+ double period) {
+ this(Kp, Ki, Kd, 0.0, source, output, period);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+ this(Kp, Ki, Kd, source, output, kDefaultPeriod);
+ }
+
+ /**
+ * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param Kf the feed forward term
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output percentage
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source,
+ PIDOutput output) {
+ this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ m_controlLoop.close();
+ m_thisMutex.lock();
+ try {
+ m_pidOutput = null;
+ m_pidInput = null;
+ m_controlLoop = null;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Begin running the PIDController.
+ */
+ @Override
+ public void enable() {
+ m_thisMutex.lock();
+ try {
+ m_enabled = true;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Stop running the PIDController, this sets the output to zero before stopping.
+ */
+ @Override
+ public void disable() {
+ // Ensures m_enabled check and pidWrite() call occur atomically
+ m_pidWriteMutex.lock();
+ try {
+ m_thisMutex.lock();
+ try {
+ m_enabled = false;
+ } finally {
+ m_thisMutex.unlock();
+ }
+
+ m_pidOutput.pidWrite(0);
+ } finally {
+ m_pidWriteMutex.unlock();
+ }
+ }
+
+ /**
+ * Set the enabled state of the PIDController.
+ */
+ public void setEnabled(boolean enable) {
+ if (enable) {
+ enable();
+ } else {
+ disable();
+ }
+ }
+
+ /**
+ * Return true if PIDController is enabled.
+ */
+ public boolean isEnabled() {
+ m_thisMutex.lock();
+ try {
+ return m_enabled;
+ } finally {
+ m_thisMutex.unlock();
+ }
+ }
+
+ /**
+ * Reset the previous error, the integral term, and disable the controller.
+ */
+ @Override
+ public void reset() {
+ disable();
+
+ super.reset();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ super.initSendable(builder);
+ builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
new file mode 100644
index 0000000..fd91ec8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+@SuppressWarnings("SummaryJavadoc")
+public interface PIDInterface {
+ @SuppressWarnings("ParameterName")
+ void setPID(double p, double i, double d);
+
+ double getP();
+
+ double getI();
+
+ double getD();
+
+ void setSetpoint(double setpoint);
+
+ double getSetpoint();
+
+ double getError();
+
+ void reset();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
new file mode 100644
index 0000000..0ef9403
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows PIDController to write it's results to its output.
+ */
+@FunctionalInterface
+public interface PIDOutput {
+ /**
+ * Set the output to the value calculated by PIDController.
+ *
+ * @param output the value calculated by PIDController
+ */
+ void pidWrite(double output);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
new file mode 100644
index 0000000..841a232
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows for PIDController to automatically read from this object.
+ */
+public interface PIDSource {
+ /**
+ * Set which parameter of the device you are using as a process control variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+ void setPIDSourceType(PIDSourceType pidSource);
+
+ /**
+ * Get which parameter of the device you are using as a process control variable.
+ *
+ * @return the currently selected PID source parameter
+ */
+ PIDSourceType getPIDSourceType();
+
+ /**
+ * Get the result to use in PIDController.
+ *
+ * @return the result to use in PIDController
+ */
+ double pidGet();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
new file mode 100644
index 0000000..31aa79a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * A description for the type of output value to provide to a PIDController.
+ */
+public enum PIDSourceType {
+ kDisplacement,
+ kRate
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
new file mode 100644
index 0000000..d59a413
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
@@ -0,0 +1,317 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PWMConfigDataResult;
+import edu.wpi.first.hal.PWMJNI;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class implements the PWM generation in the FPGA.
+ *
+ * <p>The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They are mapped to
+ * the hardware dependent values, in this case 0-2000 for the FPGA. Changes are immediately sent to
+ * the FPGA, and the update occurs at the next FPGA cycle (5.005ms). There is no delay.
+ *
+ * <p>As of revision 0.1.10 of the FPGA, the FPGA interprets the 0-2000 values as follows: - 2000 =
+ * maximum pulse width - 1999 to 1001 = linear scaling from "full forward" to "center" - 1000 =
+ * center value - 999 to 2 = linear scaling from "center" to "full reverse" - 1 = minimum pulse
+ * width (currently .5ms) - 0 = disabled (i.e. PWM output is held low)
+ */
+public class PWM extends MotorSafety implements Sendable, AutoCloseable {
+ /**
+ * Represents the amount to multiply the minimum servo-pulse pwm period by.
+ */
+ public enum PeriodMultiplier {
+ /**
+ * Period Multiplier: don't skip pulses. PWM pulses occur every 5.005 ms
+ */
+ k1X,
+ /**
+ * Period Multiplier: skip every other pulse. PWM pulses occur every 10.010 ms
+ */
+ k2X,
+ /**
+ * Period Multiplier: skip three out of four pulses. PWM pulses occur every 20.020 ms
+ */
+ k4X
+ }
+
+ private final int m_channel;
+ private int m_handle;
+
+ private final SendableImpl m_sendableImpl;
+
+ /**
+ * Allocate a PWM given a channel.
+ *
+ * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+ */
+ public PWM(final int channel) {
+ m_sendableImpl = new SendableImpl(true);
+
+ SensorUtil.checkPWMChannel(channel);
+ m_channel = channel;
+
+ m_handle = PWMJNI.initializePWMPort(HAL.getPort((byte) channel));
+
+ setDisabled();
+
+ PWMJNI.setPWMEliminateDeadband(m_handle, false);
+
+ HAL.report(tResourceType.kResourceType_PWM, channel);
+ setName("PWM", channel);
+
+ setSafetyEnabled(false);
+ }
+
+ /**
+ * Free the resource associated with the PWM channel and set the value to 0.
+ */
+ @Override
+ public void close() {
+ m_sendableImpl.close();
+
+ if (m_handle == 0) {
+ return;
+ }
+ setDisabled();
+ PWMJNI.freePWMPort(m_handle);
+ m_handle = 0;
+ }
+
+ @Override
+ public final synchronized String getName() {
+ return m_sendableImpl.getName();
+ }
+
+ @Override
+ public final synchronized void setName(String name) {
+ m_sendableImpl.setName(name);
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ */
+ protected final void setName(String moduleType, int channel) {
+ m_sendableImpl.setName(moduleType, channel);
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ */
+ protected final void setName(String moduleType, int moduleNumber, int channel) {
+ m_sendableImpl.setName(moduleType, moduleNumber, channel);
+ }
+
+ @Override
+ public final synchronized String getSubsystem() {
+ return m_sendableImpl.getSubsystem();
+ }
+
+ @Override
+ public final synchronized void setSubsystem(String subsystem) {
+ m_sendableImpl.setSubsystem(subsystem);
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ protected final void addChild(Object child) {
+ m_sendableImpl.addChild(child);
+ }
+
+ @Override
+ public void stopMotor() {
+ setDisabled();
+ }
+
+ @Override
+ public String getDescription() {
+ return "PWM " + getChannel();
+ }
+
+ /**
+ * Optionally eliminate the deadband from a speed controller.
+ *
+ * @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate the deadband
+ * in the middle of the range. Otherwise, keep the full range without
+ * modifying any values.
+ */
+ public void enableDeadbandElimination(boolean eliminateDeadband) {
+ PWMJNI.setPWMEliminateDeadband(m_handle, eliminateDeadband);
+ }
+
+ /**
+ * Set the bounds on the PWM pulse widths. This sets the bounds on the PWM values for a particular
+ * type of controller. The values determine the upper and lower speeds as well as the deadband
+ * bracket.
+ *
+ * @param max The max PWM pulse width in ms
+ * @param deadbandMax The high end of the deadband range pulse width in ms
+ * @param center The center (off) pulse width in ms
+ * @param deadbandMin The low end of the deadband pulse width in ms
+ * @param min The minimum pulse width in ms
+ */
+ public void setBounds(double max, double deadbandMax, double center, double deadbandMin,
+ double min) {
+ PWMJNI.setPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min);
+ }
+
+ /**
+ * Gets the bounds on the PWM pulse widths. This Gets the bounds on the PWM values for a
+ * particular type of controller. The values determine the upper and lower speeds as well
+ * as the deadband bracket.
+ */
+ public PWMConfigDataResult getRawBounds() {
+ return PWMJNI.getPWMConfigRaw(m_handle);
+ }
+
+ /**
+ * Gets the channel number associated with the PWM Object.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ /**
+ * Set the PWM value based on a position.
+ *
+ * <p>This is intended to be used by servos.
+ *
+ * @param pos The position to set the servo between 0.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public void setPosition(double pos) {
+ PWMJNI.setPWMPosition(m_handle, pos);
+ }
+
+ /**
+ * Get the PWM value in terms of a position.
+ *
+ * <p>This is intended to be used by servos.
+ *
+ * @return The position the servo is set to between 0.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public double getPosition() {
+ return PWMJNI.getPWMPosition(m_handle);
+ }
+
+ /**
+ * Set the PWM value based on a speed.
+ *
+ * <p>This is intended to be used by speed controllers.
+ *
+ * @param speed The speed to set the speed controller between -1.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetCenterPwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public void setSpeed(double speed) {
+ PWMJNI.setPWMSpeed(m_handle, speed);
+ }
+
+ /**
+ * Get the PWM value in terms of speed.
+ *
+ * <p>This is intended to be used by speed controllers.
+ *
+ * @return The most recently set speed between -1.0 and 1.0.
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ */
+ public double getSpeed() {
+ return PWMJNI.getPWMSpeed(m_handle);
+ }
+
+ /**
+ * Set the PWM value directly to the hardware.
+ *
+ * <p>Write a raw value to a PWM channel.
+ *
+ * @param value Raw PWM value. Range 0 - 255.
+ */
+ public void setRaw(int value) {
+ PWMJNI.setPWMRaw(m_handle, (short) value);
+ }
+
+ /**
+ * Get the PWM value directly from the hardware.
+ *
+ * <p>Read a raw value from a PWM channel.
+ *
+ * @return Raw PWM control value. Range: 0 - 255.
+ */
+ public int getRaw() {
+ return PWMJNI.getPWMRaw(m_handle);
+ }
+
+ /**
+ * Temporarily disables the PWM output. The next set call will reenable
+ * the output.
+ */
+ public void setDisabled() {
+ PWMJNI.setPWMDisabled(m_handle);
+ }
+
+ /**
+ * Slow down the PWM signal for old devices.
+ *
+ * @param mult The period multiplier to apply to this channel
+ */
+ public void setPeriodMultiplier(PeriodMultiplier mult) {
+ switch (mult) {
+ case k4X:
+ // Squelch 3 out of 4 outputs
+ PWMJNI.setPWMPeriodScale(m_handle, 3);
+ break;
+ case k2X:
+ // Squelch 1 out of 2 outputs
+ PWMJNI.setPWMPeriodScale(m_handle, 1);
+ break;
+ case k1X:
+ // Don't squelch any outputs
+ PWMJNI.setPWMPeriodScale(m_handle, 0);
+ break;
+ default:
+ // Cannot hit this, limited by PeriodMultiplier enum
+ }
+ }
+
+ protected void setZeroLatch() {
+ PWMJNI.latchPWMZero(m_handle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PWM");
+ builder.setActuator(true);
+ builder.setSafeState(this::setDisabled);
+ builder.addDoubleProperty("Value", this::getRaw, value -> setRaw((int) value));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
new file mode 100644
index 0000000..0a33423
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Common base class for all PWM Speed Controllers.
+ */
+public abstract class PWMSpeedController extends PWM implements SpeedController {
+ private boolean m_isInverted;
+
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ protected PWMSpeedController(int channel) {
+ super(channel);
+ }
+
+ @Override
+ public String getDescription() {
+ return "PWM " + getChannel();
+ }
+
+ /**
+ * Set the PWM value.
+ *
+ * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+ * FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+ @Override
+ public void set(double speed) {
+ setSpeed(m_isInverted ? -speed : speed);
+ feed();
+ }
+
+ /**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+ @Override
+ public double get() {
+ return getSpeed();
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ @Override
+ public void disable() {
+ setDisabled();
+ }
+
+ /**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Speed Controller");
+ builder.setActuator(true);
+ builder.setSafeState(this::setDisabled);
+ builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
new file mode 100644
index 0000000..2b42ed4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control.
+ */
+public class PWMTalonSRX extends PWMSpeedController {
+ /**
+ * Constructor for a PWMTalonSRX connected via PWM.
+ *
+ * <p>Note that the PWMTalonSRX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the TalonSRX User Manual
+ * available from CTRE.
+ *
+ * <p>- 2.0004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
+ * center
+ * of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms = full
+ * "reverse"
+ *
+ * @param channel The PWM channel that the PWMTalonSRX is attached to. 0-9 are on-board, 10-19 are
+ * on the MXP port
+ */
+ public PWMTalonSRX(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, .997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel());
+ setName("PWMTalonSRX", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
new file mode 100644
index 0000000..f1acb43
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM control.
+ */
+public class PWMVictorSPX extends PWMSpeedController {
+ /**
+ * Constructor for a PWMVictorSPX connected via PWM.
+ *
+ * <p>Note that the PWMVictorSPX uses the following bounds for PWM values. These values should
+ * work reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the VictorSPX User
+ * Manual available from CTRE.
+ *
+ * <p>- 2.0004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
+ * center
+ * of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms = full
+ * "reverse"
+ *
+ * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9 are on-board, 10-19
+ * are on the MXP port
+ */
+ public PWMVictorSPX(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, .997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel());
+ setName("PWMVictorSPX", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
new file mode 100644
index 0000000..dd175eb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PDPJNI;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the Power Distribution
+ * Panel over CAN.
+ */
+public class PowerDistributionPanel extends SendableBase {
+ private final int m_handle;
+
+ /**
+ * Constructor.
+ *
+ * @param module The CAN ID of the PDP
+ */
+ public PowerDistributionPanel(int module) {
+ SensorUtil.checkPDPModule(module);
+ m_handle = PDPJNI.initializePDP(module);
+
+ HAL.report(tResourceType.kResourceType_PDP, module);
+ setName("PowerDistributionPanel", module);
+ }
+
+ /**
+ * Constructor. Uses the default CAN ID (0).
+ */
+ public PowerDistributionPanel() {
+ this(0);
+ }
+
+ /**
+ * Query the input voltage of the PDP.
+ *
+ * @return The voltage of the PDP in volts
+ */
+ public double getVoltage() {
+ return PDPJNI.getPDPVoltage(m_handle);
+ }
+
+ /**
+ * Query the temperature of the PDP.
+ *
+ * @return The temperature of the PDP in degrees Celsius
+ */
+ public double getTemperature() {
+ return PDPJNI.getPDPTemperature(m_handle);
+ }
+
+ /**
+ * Query the current of a single channel of the PDP.
+ *
+ * @return The current of one of the PDP channels (channels 0-15) in Amperes
+ */
+ public double getCurrent(int channel) {
+ double current = PDPJNI.getPDPChannelCurrent((byte) channel, m_handle);
+
+ SensorUtil.checkPDPChannel(channel);
+
+ return current;
+ }
+
+ /**
+ * Query the current of all monitored PDP channels (0-15).
+ *
+ * @return The current of all the channels in Amperes
+ */
+ public double getTotalCurrent() {
+ return PDPJNI.getPDPTotalCurrent(m_handle);
+ }
+
+ /**
+ * Query the total power drawn from the monitored PDP channels.
+ *
+ * @return the total power in Watts
+ */
+ public double getTotalPower() {
+ return PDPJNI.getPDPTotalPower(m_handle);
+ }
+
+ /**
+ * Query the total energy drawn from the monitored PDP channels.
+ *
+ * @return the total energy in Joules
+ */
+ public double getTotalEnergy() {
+ return PDPJNI.getPDPTotalEnergy(m_handle);
+ }
+
+ /**
+ * Reset the total energy to 0.
+ */
+ public void resetTotalEnergy() {
+ PDPJNI.resetPDPTotalEnergy(m_handle);
+ }
+
+ /**
+ * Clear all PDP sticky faults.
+ */
+ public void clearStickyFaults() {
+ PDPJNI.clearPDPStickyFaults(m_handle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("PowerDistributionPanel");
+ for (int i = 0; i < SensorUtil.kPDPChannels; ++i) {
+ final int chan = i;
+ builder.addDoubleProperty("Chan" + i, () -> getCurrent(chan), null);
+ }
+ builder.addDoubleProperty("Voltage", this::getVoltage, null);
+ builder.addDoubleProperty("TotalCurrent", this::getTotalCurrent, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
new file mode 100644
index 0000000..45d9e0e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
@@ -0,0 +1,259 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Vector;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * The preferences class provides a relatively simple way to save important values to the roboRIO to
+ * access the next time the roboRIO is booted.
+ *
+ * <p> This class loads and saves from a file inside the roboRIO. The user can not access the file
+ * directly, but may modify values at specific fields which will then be automatically saved to the
+ * file by the NetworkTables server. </p>
+ *
+ * <p> This class is thread safe. </p>
+ *
+ * <p> This will also interact with {@link NetworkTable} by creating a table called "Preferences"
+ * with all the key-value pairs. </p>
+ */
+public final class Preferences {
+ /**
+ * The Preferences table name.
+ */
+ private static final String TABLE_NAME = "Preferences";
+ /**
+ * The singleton instance.
+ */
+ private static Preferences instance;
+ /**
+ * The network table.
+ */
+ private final NetworkTable m_table;
+
+ /**
+ * Returns the preferences instance.
+ *
+ * @return the preferences instance
+ */
+ public static synchronized Preferences getInstance() {
+ if (instance == null) {
+ instance = new Preferences();
+ }
+ return instance;
+ }
+
+ /**
+ * Creates a preference class.
+ */
+ private Preferences() {
+ m_table = NetworkTableInstance.getDefault().getTable(TABLE_NAME);
+ m_table.getEntry(".type").setString("RobotPreferences");
+ // Listener to set all Preferences values to persistent
+ // (for backwards compatibility with old dashboards).
+ m_table.addEntryListener(
+ (table, key, entry, value, flags) -> entry.setPersistent(),
+ EntryListenerFlags.kImmediate | EntryListenerFlags.kNew);
+ HAL.report(tResourceType.kResourceType_Preferences, 0);
+ }
+
+ /**
+ * Gets the vector of keys.
+ * @return a vector of the keys
+ */
+ @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+ public Vector<String> getKeys() {
+ return new Vector<>(m_table.getKeys());
+ }
+
+ /**
+ * Puts the given string into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ * @throws NullPointerException if value is null
+ */
+ public void putString(String key, String value) {
+ requireNonNull(value, "Provided value was null");
+
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setString(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given int into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putInt(String key, int value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given double into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putDouble(String key, double value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given float into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putFloat(String key, float value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given boolean into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putBoolean(String key, boolean value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setBoolean(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Puts the given long into the preferences table.
+ *
+ * @param key the key
+ * @param value the value
+ */
+ public void putLong(String key, long value) {
+ NetworkTableEntry entry = m_table.getEntry(key);
+ entry.setDouble(value);
+ entry.setPersistent();
+ }
+
+ /**
+ * Returns whether or not there is a key with the given name.
+ *
+ * @param key the key
+ * @return if there is a value at the given key
+ */
+ public boolean containsKey(String key) {
+ return m_table.containsKey(key);
+ }
+
+ /**
+ * Remove a preference.
+ *
+ * @param key the key
+ */
+ public void remove(String key) {
+ m_table.delete(key);
+ }
+
+ /**
+ * Remove all preferences.
+ */
+ public void removeAll() {
+ for (String key : m_table.getKeys()) {
+ if (!".type".equals(key)) {
+ remove(key);
+ }
+ }
+ }
+
+ /**
+ * Returns the string at the given key. If this table does not have a value for that position,
+ * then the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public String getString(String key, String backup) {
+ return m_table.getEntry(key).getString(backup);
+ }
+
+ /**
+ * Returns the int at the given key. If this table does not have a value for that position, then
+ * the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public int getInt(String key, int backup) {
+ return (int) m_table.getEntry(key).getDouble(backup);
+ }
+
+ /**
+ * Returns the double at the given key. If this table does not have a value for that position,
+ * then the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public double getDouble(String key, double backup) {
+ return m_table.getEntry(key).getDouble(backup);
+ }
+
+ /**
+ * Returns the boolean at the given key. If this table does not have a value for that position,
+ * then the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public boolean getBoolean(String key, boolean backup) {
+ return m_table.getEntry(key).getBoolean(backup);
+ }
+
+ /**
+ * Returns the float at the given key. If this table does not have a value for that position, then
+ * the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public float getFloat(String key, float backup) {
+ return (float) m_table.getEntry(key).getDouble(backup);
+ }
+
+ /**
+ * Returns the long at the given key. If this table does not have a value for that position, then
+ * the given backup value will be returned.
+ *
+ * @param key the key
+ * @param backup the value to return if none exists in the table
+ * @return either the value in the table, or the backup
+ */
+ public long getLong(String key, long backup) {
+ return (long) m_table.getEntry(key).getDouble(backup);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
new file mode 100644
index 0000000..549a681
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -0,0 +1,368 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Arrays;
+import java.util.Optional;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.RelayJNI;
+import edu.wpi.first.hal.util.UncleanStatusException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Class for VEX Robotics Spike style relay outputs. Relays are intended to be connected to Spikes
+ * or similar relays. The relay channels controls a pair of channels that are either both off, one
+ * on, the other on, or both on. This translates into two Spike outputs at 0v, one at 12v and one
+ * at 0v, one at 0v and the other at 12v, or two Spike outputs at 12V. This allows off, full
+ * forward, or full reverse control of motors without variable speed. It also allows the two
+ * channels (forward and reverse) to be used independently for something that does not care about
+ * voltage polarity (like a solenoid).
+ */
+public class Relay extends MotorSafety implements Sendable, AutoCloseable {
+ /**
+ * This class represents errors in trying to set relay values contradictory to the direction to
+ * which the relay is set.
+ */
+ public static class InvalidValueException extends RuntimeException {
+ /**
+ * Create a new exception with the given message.
+ *
+ * @param message the message to pass with the exception
+ */
+ public InvalidValueException(String message) {
+ super(message);
+ }
+ }
+
+ /**
+ * The state to drive a Relay to.
+ */
+ public enum Value {
+ kOff("Off"),
+ kOn("On"),
+ kForward("Forward"),
+ kReverse("Reverse");
+
+ private final String m_prettyValue;
+
+ Value(String prettyValue) {
+ m_prettyValue = prettyValue;
+ }
+
+ public String getPrettyValue() {
+ return m_prettyValue;
+ }
+
+ public static Optional<Value> getValueOf(String value) {
+ return Arrays.stream(Value.values()).filter(v -> v.m_prettyValue.equals(value)).findFirst();
+ }
+ }
+
+ /**
+ * The Direction(s) that a relay is configured to operate in.
+ */
+ public enum Direction {
+ /**
+ * direction: both directions are valid.
+ */
+
+ kBoth,
+ /**
+ * direction: Only forward is valid.
+ */
+ kForward,
+ /**
+ * direction: only reverse is valid.
+ */
+ kReverse
+ }
+
+ private final int m_channel;
+
+ private int m_forwardHandle;
+ private int m_reverseHandle;
+
+ private Direction m_direction;
+
+ private final SendableImpl m_sendableImpl;
+
+ /**
+ * Common relay initialization method. This code is common to all Relay constructors and
+ * initializes the relay and reserves all resources that need to be locked. Initially the relay is
+ * set to both lines at 0v.
+ */
+ private void initRelay() {
+ SensorUtil.checkRelayChannel(m_channel);
+
+ int portHandle = HAL.getPort((byte) m_channel);
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ m_forwardHandle = RelayJNI.initializeRelayPort(portHandle, true);
+ HAL.report(tResourceType.kResourceType_Relay, m_channel);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ m_reverseHandle = RelayJNI.initializeRelayPort(portHandle, false);
+ HAL.report(tResourceType.kResourceType_Relay, m_channel + 128);
+ }
+
+ setSafetyEnabled(false);
+
+ setName("Relay", m_channel);
+ }
+
+ /**
+ * Relay constructor given a channel.
+ *
+ * @param channel The channel number for this relay (0 - 3).
+ * @param direction The direction that the Relay object will control.
+ */
+ public Relay(final int channel, Direction direction) {
+ m_sendableImpl = new SendableImpl(true);
+
+ m_channel = channel;
+ m_direction = requireNonNull(direction, "Null Direction was given");
+ initRelay();
+ set(Value.kOff);
+ }
+
+ /**
+ * Relay constructor given a channel, allowing both directions.
+ *
+ * @param channel The channel number for this relay (0 - 3).
+ */
+ public Relay(final int channel) {
+ this(channel, Direction.kBoth);
+ }
+
+ @Override
+ public void close() {
+ m_sendableImpl.close();
+ freeRelay();
+ }
+
+ private void freeRelay() {
+ try {
+ RelayJNI.setRelay(m_forwardHandle, false);
+ } catch (UncleanStatusException ignored) {
+ // do nothing. Ignore
+ }
+ try {
+ RelayJNI.setRelay(m_reverseHandle, false);
+ } catch (UncleanStatusException ignored) {
+ // do nothing. Ignore
+ }
+
+ RelayJNI.freeRelayPort(m_forwardHandle);
+ RelayJNI.freeRelayPort(m_reverseHandle);
+
+ m_forwardHandle = 0;
+ m_reverseHandle = 0;
+ }
+
+ @Override
+ public final synchronized String getName() {
+ return m_sendableImpl.getName();
+ }
+
+ @Override
+ public final synchronized void setName(String name) {
+ m_sendableImpl.setName(name);
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ */
+ protected final void setName(String moduleType, int channel) {
+ m_sendableImpl.setName(moduleType, channel);
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ */
+ protected final void setName(String moduleType, int moduleNumber, int channel) {
+ m_sendableImpl.setName(moduleType, moduleNumber, channel);
+ }
+
+ @Override
+ public final synchronized String getSubsystem() {
+ return m_sendableImpl.getSubsystem();
+ }
+
+ @Override
+ public final synchronized void setSubsystem(String subsystem) {
+ m_sendableImpl.setSubsystem(subsystem);
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ protected final void addChild(Object child) {
+ m_sendableImpl.addChild(child);
+ }
+
+ /**
+ * Set the relay state.
+ *
+ * <p>Valid values depend on which directions of the relay are controlled by the object.
+ *
+ * <p>When set to kBothDirections, the relay can be set to any of the four states: 0v-0v, 12v-0v,
+ * 0v-12v, 12v-12v
+ *
+ * <p>When set to kForwardOnly or kReverseOnly, you can specify the constant for the direction or
+ * you can simply specify kOff and kOn. Using only kOff and kOn is recommended.
+ *
+ * @param value The state to set the relay.
+ */
+ @SuppressWarnings("PMD.CyclomaticComplexity")
+ public void set(Value value) {
+ switch (value) {
+ case kOff:
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ RelayJNI.setRelay(m_forwardHandle, false);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ RelayJNI.setRelay(m_reverseHandle, false);
+ }
+ break;
+ case kOn:
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ RelayJNI.setRelay(m_forwardHandle, true);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ RelayJNI.setRelay(m_reverseHandle, true);
+ }
+ break;
+ case kForward:
+ if (m_direction == Direction.kReverse) {
+ throw new InvalidValueException("A relay configured for reverse cannot be set to "
+ + "forward");
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
+ RelayJNI.setRelay(m_forwardHandle, true);
+ }
+ if (m_direction == Direction.kBoth) {
+ RelayJNI.setRelay(m_reverseHandle, false);
+ }
+ break;
+ case kReverse:
+ if (m_direction == Direction.kForward) {
+ throw new InvalidValueException("A relay configured for forward cannot be set to "
+ + "reverse");
+ }
+ if (m_direction == Direction.kBoth) {
+ RelayJNI.setRelay(m_forwardHandle, false);
+ }
+ if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
+ RelayJNI.setRelay(m_reverseHandle, true);
+ }
+ break;
+ default:
+ // Cannot hit this, limited by Value enum
+ }
+ }
+
+ /**
+ * Get the Relay State.
+ *
+ * <p>Gets the current state of the relay.
+ *
+ * <p>When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
+ * kForward/kReverse (per the recommendation in Set)
+ *
+ * @return The current state of the relay as a Relay::Value
+ */
+ public Value get() {
+ if (m_direction == Direction.kForward) {
+ if (RelayJNI.getRelay(m_forwardHandle)) {
+ return Value.kOn;
+ } else {
+ return Value.kOff;
+ }
+ } else if (m_direction == Direction.kReverse) {
+ if (RelayJNI.getRelay(m_reverseHandle)) {
+ return Value.kOn;
+ } else {
+ return Value.kOff;
+ }
+ } else {
+ if (RelayJNI.getRelay(m_forwardHandle)) {
+ if (RelayJNI.getRelay(m_reverseHandle)) {
+ return Value.kOn;
+ } else {
+ return Value.kForward;
+ }
+ } else {
+ if (RelayJNI.getRelay(m_reverseHandle)) {
+ return Value.kReverse;
+ } else {
+ return Value.kOff;
+ }
+ }
+ }
+ }
+
+ /**
+ * Get the channel number.
+ *
+ * @return The channel number.
+ */
+ public int getChannel() {
+ return m_channel;
+ }
+
+ @Override
+ public void stopMotor() {
+ set(Value.kOff);
+ }
+
+ @Override
+ public String getDescription() {
+ return "Relay ID " + getChannel();
+ }
+
+ /**
+ * Set the Relay Direction.
+ *
+ * <p>Changes which values the relay can be set to depending on which direction is used
+ *
+ * <p>Valid inputs are kBothDirections, kForwardOnly, and kReverseOnly
+ *
+ * @param direction The direction for the relay to operate in
+ */
+ public void setDirection(Direction direction) {
+ requireNonNull(direction, "Null Direction was given");
+ if (m_direction == direction) {
+ return;
+ }
+
+ freeRelay();
+ m_direction = direction;
+ initRelay();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Relay");
+ builder.setActuator(true);
+ builder.setSafeState(() -> set(Value.kOff));
+ builder.addStringProperty("Value", () -> get().getPrettyValue(),
+ value -> set(Value.getValueOf(value).orElse(Value.kOff)));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
new file mode 100644
index 0000000..39a0eac
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.hal.util.CheckedAllocationException;
+
+/**
+ * Track resources in the program. The Resource class is a convenient way of keeping track of
+ * allocated arbitrary resources in the program. Resources are just indices that have an lower and
+ * upper bound that are tracked by this class. In the library they are used for tracking allocation
+ * of hardware channels but this is purely arbitrary. The resource class does not do any actual
+ * allocation, but simply tracks if a given index is currently in use.
+ *
+ * <p><b>WARNING:</b> this should only be statically allocated. When the program loads into memory
+ * all the static constructors are called. At that time a linked list of all the "Resources" is
+ * created. Then when the program actually starts - in the Robot constructor, all resources are
+ * initialized. This ensures that the program is restartable in memory without having to
+ * unload/reload.
+ */
+public final class Resource {
+ private static Resource resourceList;
+ private final boolean[] m_numAllocated;
+ private final int m_size;
+ private final Resource m_nextResource;
+
+ /**
+ * Clears all allocated resources.
+ */
+ public static void restartProgram() {
+ for (Resource r = Resource.resourceList; r != null; r = r.m_nextResource) {
+ for (int i = 0; i < r.m_size; i++) {
+ r.m_numAllocated[i] = false;
+ }
+ }
+ }
+
+ /**
+ * Allocate storage for a new instance of Resource. Allocate a bool array of values that will get
+ * initialized to indicate that no resources have been allocated yet. The indices of the
+ * resources are 0..size-1.
+ *
+ * @param size The number of blocks to allocate
+ */
+ public Resource(final int size) {
+ m_size = size;
+ m_numAllocated = new boolean[size];
+ for (int i = 0; i < size; i++) {
+ m_numAllocated[i] = false;
+ }
+ m_nextResource = Resource.resourceList;
+ Resource.resourceList = this;
+ }
+
+ /**
+ * Allocate a resource. When a resource is requested, mark it allocated. In this case, a free
+ * resource value within the range is located and returned after it is marked allocated.
+ *
+ * @return The index of the allocated block.
+ * @throws CheckedAllocationException If there are no resources available to be allocated.
+ */
+ public int allocate() throws CheckedAllocationException {
+ for (int i = 0; i < m_size; i++) {
+ if (!m_numAllocated[i]) {
+ m_numAllocated[i] = true;
+ return i;
+ }
+ }
+ throw new CheckedAllocationException("No available resources");
+ }
+
+ /**
+ * Allocate a specific resource value. The user requests a specific resource value, i.e. channel
+ * number and it is verified unallocated, then returned.
+ *
+ * @param index The resource to allocate
+ * @return The index of the allocated block
+ * @throws CheckedAllocationException If there are no resources available to be allocated.
+ */
+ public int allocate(final int index) throws CheckedAllocationException {
+ if (index >= m_size || index < 0) {
+ throw new CheckedAllocationException("Index " + index + " out of range");
+ }
+ if (m_numAllocated[index]) {
+ throw new CheckedAllocationException("Resource at index " + index + " already allocated");
+ }
+ m_numAllocated[index] = true;
+ return index;
+ }
+
+ /**
+ * Free an allocated resource. After a resource is no longer needed, for example a destructor is
+ * called for a channel assignment class, Free will release the resource value so it can be reused
+ * somewhere else in the program.
+ *
+ * @param index The index of the resource to free.
+ */
+ public void free(final int index) {
+ if (!m_numAllocated[index]) {
+ throw new AllocationException("No resource available to be freed");
+ }
+ m_numAllocated[index] = false;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
new file mode 100644
index 0000000..44c03a3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -0,0 +1,285 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.util.function.Supplier;
+
+import edu.wpi.cscore.CameraServerJNI;
+import edu.wpi.first.cameraserver.CameraServerShared;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.util.WPILibVersion;
+
+/**
+ * Implement a Robot Program framework. The RobotBase class is intended to be subclassed by a user
+ * creating a robot program. Overridden autonomous() and operatorControl() methods are called at the
+ * appropriate time as the match proceeds. In the current implementation, the Autonomous code will
+ * run to completion before the OperatorControl code could start. In the future the Autonomous code
+ * might be spawned as a task, then killed at the end of the Autonomous period.
+ */
+public abstract class RobotBase implements AutoCloseable {
+ /**
+ * The ID of the main Java thread.
+ */
+ // This is usually 1, but it is best to make sure
+ public static final long MAIN_THREAD_ID = Thread.currentThread().getId();
+
+ private static void setupCameraServerShared() {
+ CameraServerShared shared = new CameraServerShared() {
+
+ @Override
+ public void reportVideoServer(int id) {
+ HAL.report(tResourceType.kResourceType_PCVideoServer, id);
+ }
+
+ @Override
+ public void reportUsbCamera(int id) {
+ HAL.report(tResourceType.kResourceType_UsbCamera, id);
+ }
+
+ @Override
+ public void reportDriverStationError(String error) {
+ DriverStation.reportError(error, true);
+ }
+
+ @Override
+ public void reportAxisCamera(int id) {
+ HAL.report(tResourceType.kResourceType_AxisCamera, id);
+ }
+
+ @Override
+ public Long getRobotMainThreadId() {
+ return MAIN_THREAD_ID;
+ }
+ };
+
+ CameraServerSharedStore.setCameraServerShared(shared);
+ }
+
+ protected final DriverStation m_ds;
+
+ /**
+ * Constructor for a generic robot program. User code should be placed in the constructor that
+ * runs before the Autonomous or Operator Control period starts. The constructor will run to
+ * completion before Autonomous is entered.
+ *
+ * <p>This must be used to ensure that the communications code starts. In the future it would be
+ * nice
+ * to put this code into it's own task that loads on boot so ensure that it runs.
+ */
+ protected RobotBase() {
+ NetworkTableInstance inst = NetworkTableInstance.getDefault();
+ setupCameraServerShared();
+ inst.setNetworkIdentity("Robot");
+ inst.startServer("/home/lvuser/networktables.ini");
+ m_ds = DriverStation.getInstance();
+ inst.getTable("LiveWindow").getSubTable(".status").getEntry("LW Enabled").setBoolean(false);
+
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ }
+
+ @Deprecated
+ public void free() {
+ }
+
+ @Override
+ public void close() {
+ }
+
+ /**
+ * Get if the robot is a simulation.
+ *
+ * @return If the robot is running in simulation.
+ */
+ public static boolean isSimulation() {
+ return !isReal();
+ }
+
+ /**
+ * Get if the robot is real.
+ *
+ * @return If the robot is running in the real world.
+ */
+ public static boolean isReal() {
+ return HALUtil.getHALRuntimeType() == 0;
+ }
+
+ /**
+ * Determine if the Robot is currently disabled.
+ *
+ * @return True if the Robot is currently disabled by the field controls.
+ */
+ public boolean isDisabled() {
+ return m_ds.isDisabled();
+ }
+
+ /**
+ * Determine if the Robot is currently enabled.
+ *
+ * @return True if the Robot is currently enabled by the field controls.
+ */
+ public boolean isEnabled() {
+ return m_ds.isEnabled();
+ }
+
+ /**
+ * Determine if the robot is currently in Autonomous mode as determined by the field
+ * controls.
+ *
+ * @return True if the robot is currently operating Autonomously.
+ */
+ public boolean isAutonomous() {
+ return m_ds.isAutonomous();
+ }
+
+ /**
+ * Determine if the robot is currently in Test mode as determined by the driver
+ * station.
+ *
+ * @return True if the robot is currently operating in Test mode.
+ */
+ public boolean isTest() {
+ return m_ds.isTest();
+ }
+
+ /**
+ * Determine if the robot is currently in Operator Control mode as determined by the field
+ * controls.
+ *
+ * @return True if the robot is currently operating in Tele-Op mode.
+ */
+ public boolean isOperatorControl() {
+ return m_ds.isOperatorControl();
+ }
+
+ /**
+ * Indicates if new data is available from the driver station.
+ *
+ * @return Has new data arrived over the network since the last time this function was called?
+ */
+ public boolean isNewDataAvailable() {
+ return m_ds.isNewControlData();
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ public abstract void startCompetition();
+
+ @SuppressWarnings("JavadocMethod")
+ public static boolean getBooleanProperty(String name, boolean defaultValue) {
+ String propVal = System.getProperty(name);
+ if (propVal == null) {
+ return defaultValue;
+ }
+ if ("false".equalsIgnoreCase(propVal)) {
+ return false;
+ } else if ("true".equalsIgnoreCase(propVal)) {
+ return true;
+ } else {
+ throw new IllegalStateException(propVal);
+ }
+ }
+
+ /**
+ * Starting point for the applications.
+ */
+ @SuppressWarnings({"PMD.AvoidInstantiatingObjectsInLoops", "PMD.AvoidCatchingThrowable",
+ "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
+ if (!HAL.initialize(500, 0)) {
+ throw new IllegalStateException("Failed to initialize. Terminating");
+ }
+
+ // Call a CameraServer JNI function to force OpenCV native library loading
+ // Needed because all the OpenCV JNI functions don't have built in loading
+ CameraServerJNI.enumerateSinks();
+
+ HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Java);
+
+ System.out.println("********** Robot program starting **********");
+
+ T robot;
+ try {
+ robot = robotSupplier.get();
+ } catch (Throwable throwable) {
+ Throwable cause = throwable.getCause();
+ if (cause != null) {
+ throwable = cause;
+ }
+ String robotName = "Unknown";
+ StackTraceElement[] elements = throwable.getStackTrace();
+ if (elements.length > 0) {
+ robotName = elements[0].getClassName();
+ }
+ DriverStation.reportError("Unhandled exception instantiating robot " + robotName + " "
+ + throwable.toString(), elements);
+ DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+ DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
+ System.exit(1);
+ return;
+ }
+
+ if (isReal()) {
+ try {
+ final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini");
+
+ if (file.exists()) {
+ file.delete();
+ }
+
+ file.createNewFile();
+
+ try (OutputStream output = Files.newOutputStream(file.toPath())) {
+ output.write("Java ".getBytes(StandardCharsets.UTF_8));
+ output.write(WPILibVersion.Version.getBytes(StandardCharsets.UTF_8));
+ }
+
+ } catch (IOException ex) {
+ DriverStation.reportError("Could not write FRC_Lib_Version.ini: " + ex.toString(),
+ ex.getStackTrace());
+ }
+ }
+
+ boolean errorOnExit = false;
+ try {
+ robot.startCompetition();
+ } catch (Throwable throwable) {
+ Throwable cause = throwable.getCause();
+ if (cause != null) {
+ throwable = cause;
+ }
+ DriverStation.reportError("Unhandled exception: " + throwable.toString(),
+ throwable.getStackTrace());
+ errorOnExit = true;
+ } finally {
+ // startCompetition never returns unless exception occurs....
+ DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+ if (errorOnExit) {
+ DriverStation.reportError(
+ "The startCompetition() method (or methods called by it) should have "
+ + "handled the exception above.", false);
+ } else {
+ DriverStation.reportError("Unexpected return from startCompetition() method.", false);
+ }
+ }
+ System.exit(1);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
new file mode 100644
index 0000000..0623812
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.hal.PowerJNI;
+import edu.wpi.first.hal.can.CANJNI;
+import edu.wpi.first.hal.can.CANStatus;
+
+/**
+ * Contains functions for roboRIO functionality.
+ */
+public final class RobotController {
+ private RobotController() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Return the FPGA Version number. For now, expect this to be the current
+ * year.
+ *
+ * @return FPGA Version number.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static int getFPGAVersion() {
+ return HALUtil.getFPGAVersion();
+ }
+
+ /**
+ * Return the FPGA Revision number. The format of the revision is 3 numbers. The 12 most
+ * significant bits are the Major Revision. the next 8 bits are the Minor Revision. The 12 least
+ * significant bits are the Build Number.
+ *
+ * @return FPGA Revision number.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static long getFPGARevision() {
+ return (long) HALUtil.getFPGARevision();
+ }
+
+ /**
+ * Read the microsecond timer from the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA.
+ */
+ public static long getFPGATime() {
+ return HALUtil.getFPGATime();
+ }
+
+ /**
+ * Get the state of the "USER" button on the roboRIO.
+ *
+ * @return true if the button is currently pressed down
+ */
+ public static boolean getUserButton() {
+ return HALUtil.getFPGAButton();
+ }
+
+ /**
+ * Read the battery voltage.
+ *
+ * @return The battery voltage in Volts.
+ */
+ public static double getBatteryVoltage() {
+ return PowerJNI.getVinVoltage();
+ }
+
+ /**
+ * Gets a value indicating whether the FPGA outputs are enabled. The outputs may be disabled if
+ * the robot is disabled or e-stopped, the watchdog has expired, or if the roboRIO browns out.
+ *
+ * @return True if the FPGA outputs are enabled.
+ */
+ public static boolean isSysActive() {
+ return HAL.getSystemActive();
+ }
+
+ /**
+ * Check if the system is browned out.
+ *
+ * @return True if the system is browned out
+ */
+ public static boolean isBrownedOut() {
+ return HAL.getBrownedOut();
+ }
+
+ /**
+ * Get the input voltage to the robot controller.
+ *
+ * @return The controller input voltage value in Volts
+ */
+ public static double getInputVoltage() {
+ return PowerJNI.getVinVoltage();
+ }
+
+ /**
+ * Get the input current to the robot controller.
+ *
+ * @return The controller input current value in Amps
+ */
+ public static double getInputCurrent() {
+ return PowerJNI.getVinCurrent();
+ }
+
+ /**
+ * Get the voltage of the 3.3V rail.
+ *
+ * @return The controller 3.3V rail voltage value in Volts
+ */
+ public static double getVoltage3V3() {
+ return PowerJNI.getUserVoltage3V3();
+ }
+
+ /**
+ * Get the current output of the 3.3V rail.
+ *
+ * @return The controller 3.3V rail output current value in Volts
+ */
+ public static double getCurrent3V3() {
+ return PowerJNI.getUserCurrent3V3();
+ }
+
+ /**
+ * Get the enabled state of the 3.3V rail. The rail may be disabled due to a controller brownout,
+ * a short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 3.3V rail enabled value
+ */
+ public static boolean getEnabled3V3() {
+ return PowerJNI.getUserActive3V3();
+ }
+
+ /**
+ * Get the count of the total current faults on the 3.3V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ public static int getFaultCount3V3() {
+ return PowerJNI.getUserCurrentFaults3V3();
+ }
+
+ /**
+ * Get the voltage of the 5V rail.
+ *
+ * @return The controller 5V rail voltage value in Volts
+ */
+ public static double getVoltage5V() {
+ return PowerJNI.getUserVoltage5V();
+ }
+
+ /**
+ * Get the current output of the 5V rail.
+ *
+ * @return The controller 5V rail output current value in Amps
+ */
+ public static double getCurrent5V() {
+ return PowerJNI.getUserCurrent5V();
+ }
+
+ /**
+ * Get the enabled state of the 5V rail. The rail may be disabled due to a controller brownout, a
+ * short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 5V rail enabled value
+ */
+ public static boolean getEnabled5V() {
+ return PowerJNI.getUserActive5V();
+ }
+
+ /**
+ * Get the count of the total current faults on the 5V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ public static int getFaultCount5V() {
+ return PowerJNI.getUserCurrentFaults5V();
+ }
+
+ /**
+ * Get the voltage of the 6V rail.
+ *
+ * @return The controller 6V rail voltage value in Volts
+ */
+ public static double getVoltage6V() {
+ return PowerJNI.getUserVoltage6V();
+ }
+
+ /**
+ * Get the current output of the 6V rail.
+ *
+ * @return The controller 6V rail output current value in Amps
+ */
+ public static double getCurrent6V() {
+ return PowerJNI.getUserCurrent6V();
+ }
+
+ /**
+ * Get the enabled state of the 6V rail. The rail may be disabled due to a controller brownout, a
+ * short circuit on the rail, or controller over-voltage.
+ *
+ * @return The controller 6V rail enabled value
+ */
+ public static boolean getEnabled6V() {
+ return PowerJNI.getUserActive6V();
+ }
+
+ /**
+ * Get the count of the total current faults on the 6V rail since the controller has booted.
+ *
+ * @return The number of faults
+ */
+ public static int getFaultCount6V() {
+ return PowerJNI.getUserCurrentFaults6V();
+ }
+
+ /**
+ * Get the current status of the CAN bus.
+ *
+ * @return The status of the CAN bus
+ */
+ public static CANStatus getCANStatus() {
+ CANStatus status = new CANStatus();
+ CANJNI.GetCANStatus(status);
+ return status;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
new file mode 100644
index 0000000..6f020fd
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
@@ -0,0 +1,723 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Utility class for handling Robot drive based on a definition of the motor configuration. The
+ * robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and mecanum
+ * drive trains are supported. In the future other drive types like swerve might be implemented.
+ * Motor channel numbers are supplied on creation of the class. Those are used for either the drive
+ * function (intended for hand created drive code, such as autonomous) or with the Tank/Arcade
+ * functions intended to be used for Operator Control driving.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.drive.DifferentialDrive}
+ * or {@link edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
+ */
+@Deprecated
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+public class RobotDrive extends MotorSafety implements AutoCloseable {
+ /**
+ * The location of a motor on the robot for the purpose of driving.
+ */
+ public enum MotorType {
+ kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ MotorType(int value) {
+ this.value = value;
+ }
+ }
+
+ public static final double kDefaultExpirationTime = 0.1;
+ public static final double kDefaultSensitivity = 0.5;
+ public static final double kDefaultMaxOutput = 1.0;
+ protected static final int kMaxNumberOfMotors = 4;
+ protected double m_sensitivity;
+ protected double m_maxOutput;
+ protected SpeedController m_frontLeftMotor;
+ protected SpeedController m_frontRightMotor;
+ protected SpeedController m_rearLeftMotor;
+ protected SpeedController m_rearRightMotor;
+ protected boolean m_allocatedSpeedControllers;
+ protected static boolean kArcadeRatioCurve_Reported;
+ protected static boolean kTank_Reported;
+ protected static boolean kArcadeStandard_Reported;
+ protected static boolean kMecanumCartesian_Reported;
+ protected static boolean kMecanumPolar_Reported;
+
+ /**
+ * Constructor for RobotDrive with 2 motors specified with channel numbers. Set up parameters for
+ * a two wheel drive system where the left and right motor pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ *
+ * @param leftMotorChannel The PWM channel number that drives the left motor.
+ * @param rightMotorChannel The PWM channel number that drives the right motor.
+ */
+ public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_frontLeftMotor = null;
+ m_rearLeftMotor = new Talon(leftMotorChannel);
+ m_frontRightMotor = null;
+ m_rearRightMotor = new Talon(rightMotorChannel);
+ m_allocatedSpeedControllers = true;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Constructor for RobotDrive with 4 motors specified with channel numbers. Set up parameters for
+ * a four wheel drive system where all four motor pwm channels are specified in the call. This
+ * call assumes Talons for controlling the motors.
+ *
+ * @param frontLeftMotor Front left motor channel number
+ * @param rearLeftMotor Rear Left motor channel number
+ * @param frontRightMotor Front right motor channel number
+ * @param rearRightMotor Rear Right motor channel number
+ */
+ public RobotDrive(final int frontLeftMotor, final int rearLeftMotor, final int frontRightMotor,
+ final int rearRightMotor) {
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_rearLeftMotor = new Talon(rearLeftMotor);
+ m_rearRightMotor = new Talon(rearRightMotor);
+ m_frontLeftMotor = new Talon(frontLeftMotor);
+ m_frontRightMotor = new Talon(frontRightMotor);
+ m_allocatedSpeedControllers = true;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Constructor for RobotDrive with 2 motors specified as SpeedController objects. The
+ * SpeedController version of the constructor enables programs to use the RobotDrive classes with
+ * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
+ * the curve to suit motor bias or dead-band elimination.
+ *
+ * @param leftMotor The left SpeedController object used to drive the robot.
+ * @param rightMotor the right SpeedController object used to drive the robot.
+ */
+ public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
+ requireNonNull(leftMotor, "Provided left motor was null");
+ requireNonNull(rightMotor, "Provided right motor was null");
+
+ m_frontLeftMotor = null;
+ m_rearLeftMotor = leftMotor;
+ m_frontRightMotor = null;
+ m_rearRightMotor = rightMotor;
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_allocatedSpeedControllers = false;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Constructor for RobotDrive with 4 motors specified as SpeedController objects. Speed controller
+ * input version of RobotDrive (see previous comments).
+ *
+ * @param frontLeftMotor The front left SpeedController object used to drive the robot
+ * @param rearLeftMotor The back left SpeedController object used to drive the robot.
+ * @param frontRightMotor The front right SpeedController object used to drive the robot.
+ * @param rearRightMotor The back right SpeedController object used to drive the robot.
+ */
+ public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
+ SpeedController frontRightMotor, SpeedController rearRightMotor) {
+ m_frontLeftMotor = requireNonNull(frontLeftMotor, "frontLeftMotor cannot be null");
+ m_rearLeftMotor = requireNonNull(rearLeftMotor, "rearLeftMotor cannot be null");
+ m_frontRightMotor = requireNonNull(frontRightMotor, "frontRightMotor cannot be null");
+ m_rearRightMotor = requireNonNull(rearRightMotor, "rearRightMotor cannot be null");
+ m_sensitivity = kDefaultSensitivity;
+ m_maxOutput = kDefaultMaxOutput;
+ m_allocatedSpeedControllers = false;
+ setSafetyEnabled(true);
+ drive(0, 0);
+ }
+
+ /**
+ * Drive the motors at "outputMagnitude" and "curve". Both outputMagnitude and curve are -1.0 to
+ * +1.0 values, where 0.0 represents stopped and not turning. {@literal curve < 0 will turn left
+ * and curve > 0} will turn right.
+ *
+ * <p>The algorithm for steering provides a constant turn radius for any normal speed range, both
+ * forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
+ *
+ * <p>This function will most likely be used in an autonomous routine.
+ *
+ * @param outputMagnitude The speed setting for the outside wheel in a turn, forward or backwards,
+ * +1 to -1.
+ * @param curve The rate of turn, constant for different forward speeds. Set {@literal
+ * curve < 0 for left turn or curve > 0 for right turn.} Set curve =
+ * e^(-r/w) to get a turn radius r for wheelbase w of your robot.
+ * Conversely, turn radius r = -ln(curve)*w for a given value of curve and
+ * wheelbase w.
+ */
+ public void drive(double outputMagnitude, double curve) {
+ final double leftOutput;
+ final double rightOutput;
+
+ if (!kArcadeRatioCurve_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
+ tInstances.kRobotDrive_ArcadeRatioCurve);
+ kArcadeRatioCurve_Reported = true;
+ }
+ if (curve < 0) {
+ double value = Math.log(-curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) {
+ ratio = .0000000001;
+ }
+ leftOutput = outputMagnitude / ratio;
+ rightOutput = outputMagnitude;
+ } else if (curve > 0) {
+ double value = Math.log(curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) {
+ ratio = .0000000001;
+ }
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude / ratio;
+ } else {
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude;
+ }
+ setLeftRightMotorOutputs(leftOutput, rightOutput);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. drive the robot using two joystick
+ * inputs. The Y-axis will be selected from each Joystick object. The calculated values will be
+ * squared to decrease sensitivity at low speeds.
+ *
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ */
+ public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
+ requireNonNull(leftStick, "Provided left stick was null");
+ requireNonNull(rightStick, "Provided right stick was null");
+
+ tankDrive(leftStick.getY(), rightStick.getY(), true);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. drive the robot using two joystick
+ * inputs. The Y-axis will be selected from each Joystick object.
+ *
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
+ requireNonNull(leftStick, "Provided left stick was null");
+ requireNonNull(rightStick, "Provided right stick was null");
+
+ tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you pick the
+ * axis to be used on each Joystick object for the left and right sides of the robot. The
+ * calculated values will be squared to decrease sensitivity at low speeds.
+ *
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ */
+ public void tankDrive(GenericHID leftStick, final int leftAxis, GenericHID rightStick,
+ final int rightAxis) {
+ requireNonNull(leftStick, "Provided left stick was null");
+ requireNonNull(rightStick, "Provided right stick was null");
+
+ tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you pick the
+ * axis to be used on each Joystick object for the left and right sides of the robot.
+ *
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void tankDrive(GenericHID leftStick, final int leftAxis, GenericHID rightStick,
+ final int rightAxis, boolean squaredInputs) {
+ requireNonNull(leftStick, "Provided left stick was null");
+ requireNonNull(rightStick, "Provided right stick was null");
+
+ tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you directly
+ * provide joystick values from any source.
+ *
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
+ if (!kTank_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
+ tInstances.kRobotDrive_Tank);
+ kTank_Reported = true;
+ }
+
+ leftValue = limit(leftValue);
+ rightValue = limit(rightValue);
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (squaredInputs) {
+ leftValue = Math.copySign(leftValue * leftValue, leftValue);
+ rightValue = Math.copySign(rightValue * rightValue, rightValue);
+ }
+ setLeftRightMotorOutputs(leftValue, rightValue);
+ }
+
+ /**
+ * Provide tank steering using the stored robot configuration. This function lets you directly
+ * provide joystick values from any source. The calculated values will be squared to decrease
+ * sensitivity at low speeds.
+ *
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ */
+ public void tankDrive(double leftValue, double rightValue) {
+ tankDrive(leftValue, rightValue, true);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
+ * axis for the move value and the X axis for the rotate value. (Should add more information here
+ * regarding the way that arcade drive works.)
+ *
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be
+ * selected for forwards/backwards and the X-axis will be selected for
+ * rotation rate.
+ * @param squaredInputs If true, the sensitivity will be decreased for small values
+ */
+ public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
+ // simply call the full-featured arcadeDrive with the appropriate values
+ arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
+ * axis for the move value and the X axis for the rotate value. (Should add more information here
+ * regarding the way that arcade drive works.) The calculated values will be squared to decrease
+ * sensitivity at low speeds.
+ *
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
+ * for forwards/backwards and the X-axis will be selected for rotation rate.
+ */
+ public void arcadeDrive(GenericHID stick) {
+ arcadeDrive(stick, true);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given two joystick instances and two axis,
+ * compute the values to send to either two or four motors.
+ *
+ * @param moveStick The Joystick object that represents the forward/backward direction
+ * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
+ * Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate right/left
+ * (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+ */
+ public void arcadeDrive(GenericHID moveStick, final int moveAxis, GenericHID rotateStick,
+ final int rotateAxis, boolean squaredInputs) {
+ double moveValue = moveStick.getRawAxis(moveAxis);
+ double rotateValue = rotateStick.getRawAxis(rotateAxis);
+
+ arcadeDrive(moveValue, rotateValue, squaredInputs);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. Given two joystick instances and two axis,
+ * compute the values to send to either two or four motors. The calculated values will be
+ * squared to decrease sensitivity at low speeds.
+ *
+ * @param moveStick The Joystick object that represents the forward/backward direction
+ * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
+ * Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically
+ * X_AXIS)
+ */
+ public void arcadeDrive(GenericHID moveStick, final int moveAxis, GenericHID rotateStick,
+ final int rotateAxis) {
+ arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. This function lets you directly provide
+ * joystick values from any source.
+ *
+ * @param moveValue The value to use for forwards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ * @param squaredInputs If set, decreases the sensitivity at low speeds
+ */
+ public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
+ // local variables to hold the computed PWM values for the motors
+ if (!kArcadeStandard_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
+ tInstances.kRobotDrive_ArcadeStandard);
+ kArcadeStandard_Reported = true;
+ }
+
+ double leftMotorSpeed;
+ double rightMotorSpeed;
+
+ moveValue = limit(moveValue);
+ rotateValue = limit(rotateValue);
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (squaredInputs) {
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ moveValue = Math.copySign(moveValue * moveValue, moveValue);
+ rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
+ }
+
+ if (moveValue > 0.0) {
+ if (rotateValue > 0.0) {
+ leftMotorSpeed = moveValue - rotateValue;
+ rightMotorSpeed = Math.max(moveValue, rotateValue);
+ } else {
+ leftMotorSpeed = Math.max(moveValue, -rotateValue);
+ rightMotorSpeed = moveValue + rotateValue;
+ }
+ } else {
+ if (rotateValue > 0.0) {
+ leftMotorSpeed = -Math.max(-moveValue, rotateValue);
+ rightMotorSpeed = moveValue + rotateValue;
+ } else {
+ leftMotorSpeed = moveValue - rotateValue;
+ rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
+ }
+ }
+
+ setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
+ }
+
+ /**
+ * Arcade drive implements single stick driving. This function lets you directly provide
+ * joystick values from any source. The calculated values will be squared to decrease
+ * sensitivity at low speeds.
+ *
+ * @param moveValue The value to use for forwards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ */
+ public void arcadeDrive(double moveValue, double rotateValue) {
+ arcadeDrive(moveValue, rotateValue, true);
+ }
+
+ /**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
+ * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
+ * top, the roller axles should form an X across the robot.
+ *
+ * <p>This is designed to be directly driven by joystick axes.
+ *
+ * @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
+ * @param y The speed that the robot should drive in the Y direction. This input is
+ * inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
+ * @param rotation The rate of rotation for the robot that is completely independent of the
+ * translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented
+ * controls.
+ */
+ @SuppressWarnings("ParameterName")
+ public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
+ if (!kMecanumCartesian_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
+ tInstances.kRobotDrive_MecanumCartesian);
+ kMecanumCartesian_Reported = true;
+ }
+ @SuppressWarnings("LocalVariableName")
+ double xIn = x;
+ @SuppressWarnings("LocalVariableName")
+ double yIn = y;
+ // Negate y for the joystick.
+ yIn = -yIn;
+ // Compensate for gyro angle.
+ double[] rotated = rotateVector(xIn, yIn, gyroAngle);
+ xIn = rotated[0];
+ yIn = rotated[1];
+
+ double[] wheelSpeeds = new double[kMaxNumberOfMotors];
+ wheelSpeeds[MotorType.kFrontLeft.value] = xIn + yIn + rotation;
+ wheelSpeeds[MotorType.kFrontRight.value] = -xIn + yIn - rotation;
+ wheelSpeeds[MotorType.kRearLeft.value] = -xIn + yIn + rotation;
+ wheelSpeeds[MotorType.kRearRight.value] = xIn + yIn - rotation;
+
+ normalize(wheelSpeeds);
+ m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
+ m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
+ m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
+ m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
+ * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
+ * top, the roller axles should form an X across the robot.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
+ * @param direction The angle the robot should drive in degrees. The direction and magnitude
+ * are independent of the rotation rate. [-180.0..180.0]
+ * @param rotation The rate of rotation for the robot that is completely independent of the
+ * magnitude or direction. [-1.0..1.0]
+ */
+ public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
+ if (!kMecanumPolar_Reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
+ tInstances.kRobotDrive_MecanumPolar);
+ kMecanumPolar_Reported = true;
+ }
+ // Normalized for full power along the Cartesian axes.
+ magnitude = limit(magnitude) * Math.sqrt(2.0);
+ // The rollers are at 45 degree angles.
+ double dirInRad = (direction + 45.0) * Math.PI / 180.0;
+ double cosD = Math.cos(dirInRad);
+ double sinD = Math.sin(dirInRad);
+
+ double[] wheelSpeeds = new double[kMaxNumberOfMotors];
+ wheelSpeeds[MotorType.kFrontLeft.value] = sinD * magnitude + rotation;
+ wheelSpeeds[MotorType.kFrontRight.value] = cosD * magnitude - rotation;
+ wheelSpeeds[MotorType.kRearLeft.value] = cosD * magnitude + rotation;
+ wheelSpeeds[MotorType.kRearRight.value] = sinD * magnitude - rotation;
+
+ normalize(wheelSpeeds);
+
+ m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
+ m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
+ m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
+ m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Holonomic Drive method for Mecanum wheeled robots.
+ *
+ * <p>This is an alias to mecanumDrive_Polar() for backward compatibility
+ *
+ * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
+ * @param direction The direction the robot should drive. The direction and maginitude are
+ * independent of the rotation rate.
+ * @param rotation The rate of rotation for the robot that is completely independent of the
+ * magnitute or direction. [-1.0..1.0]
+ */
+ void holonomicDrive(double magnitude, double direction, double rotation) {
+ mecanumDrive_Polar(magnitude, direction, rotation);
+ }
+
+ /**
+ * Set the speed of the right and left motors. This is used once an appropriate drive setup
+ * function is called such as twoWheelDrive(). The motors are set to "leftSpeed" and
+ * "rightSpeed" and includes flipping the direction of one side for opposing motors.
+ *
+ * @param leftOutput The speed to send to the left side of the robot.
+ * @param rightOutput The speed to send to the right side of the robot.
+ */
+ public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
+ requireNonNull(m_rearLeftMotor, "Provided left motor was null");
+ requireNonNull(m_rearRightMotor, "Provided right motor was null");
+
+ if (m_frontLeftMotor != null) {
+ m_frontLeftMotor.set(limit(leftOutput) * m_maxOutput);
+ }
+ m_rearLeftMotor.set(limit(leftOutput) * m_maxOutput);
+
+ if (m_frontRightMotor != null) {
+ m_frontRightMotor.set(-limit(rightOutput) * m_maxOutput);
+ }
+ m_rearRightMotor.set(-limit(rightOutput) * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+ protected static double limit(double number) {
+ if (number > 1.0) {
+ return 1.0;
+ }
+ if (number < -1.0) {
+ return -1.0;
+ }
+ return number;
+ }
+
+ /**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+ protected static void normalize(double[] wheelSpeeds) {
+ double maxMagnitude = Math.abs(wheelSpeeds[0]);
+ for (int i = 1; i < kMaxNumberOfMotors; i++) {
+ double temp = Math.abs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) {
+ maxMagnitude = temp;
+ }
+ }
+ if (maxMagnitude > 1.0) {
+ for (int i = 0; i < kMaxNumberOfMotors; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+ }
+
+ /**
+ * Rotate a vector in Cartesian space.
+ */
+ @SuppressWarnings("ParameterName")
+ protected static double[] rotateVector(double x, double y, double angle) {
+ double cosA = Math.cos(angle * (Math.PI / 180.0));
+ double sinA = Math.sin(angle * (Math.PI / 180.0));
+ double[] out = new double[2];
+ out[0] = x * cosA - y * sinA;
+ out[1] = x * sinA + y * cosA;
+ return out;
+ }
+
+ /**
+ * Invert a motor direction. This is used when a motor should run in the opposite direction as the
+ * drive code would normally run it. Motors that are direct drive would be inverted, the drive
+ * code assumes that the motors are geared with one reversal.
+ *
+ * @param motor The motor index to invert.
+ * @param isInverted True if the motor should be inverted when operated.
+ */
+ public void setInvertedMotor(MotorType motor, boolean isInverted) {
+ switch (motor) {
+ case kFrontLeft:
+ m_frontLeftMotor.setInverted(isInverted);
+ break;
+ case kFrontRight:
+ m_frontRightMotor.setInverted(isInverted);
+ break;
+ case kRearLeft:
+ m_rearLeftMotor.setInverted(isInverted);
+ break;
+ case kRearRight:
+ m_rearRightMotor.setInverted(isInverted);
+ break;
+ default:
+ throw new IllegalArgumentException("Illegal motor type: " + motor);
+ }
+ }
+
+ /**
+ * Set the turning sensitivity.
+ *
+ * <p>This only impacts the drive() entry-point.
+ *
+ * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
+ */
+ public void setSensitivity(double sensitivity) {
+ m_sensitivity = sensitivity;
+ }
+
+ /**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than
+ * PercentVbus.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_maxOutput = maxOutput;
+ }
+
+ @Deprecated
+ public void free() {
+ close();
+ }
+
+ /**
+ * Free the speed controllers if they were allocated locally.
+ */
+ @Override
+ public void close() {
+ if (m_allocatedSpeedControllers) {
+ if (m_frontLeftMotor != null) {
+ ((PWM) m_frontLeftMotor).close();
+ }
+ if (m_frontRightMotor != null) {
+ ((PWM) m_frontRightMotor).close();
+ }
+ if (m_rearLeftMotor != null) {
+ ((PWM) m_rearLeftMotor).close();
+ }
+ if (m_rearRightMotor != null) {
+ ((PWM) m_rearRightMotor).close();
+ }
+ }
+ }
+
+ @Override
+ public String getDescription() {
+ return "Robot Drive";
+ }
+
+ @Override
+ public void stopMotor() {
+ if (m_frontLeftMotor != null) {
+ m_frontLeftMotor.stopMotor();
+ }
+ if (m_frontRightMotor != null) {
+ m_frontRightMotor.stopMotor();
+ }
+ if (m_rearLeftMotor != null) {
+ m_rearLeftMotor.stopMotor();
+ }
+ if (m_rearRightMotor != null) {
+ m_rearRightMotor.stopMotor();
+ }
+
+ feed();
+ }
+
+ protected int getNumMotors() {
+ int motors = 0;
+ if (m_frontLeftMotor != null) {
+ motors++;
+ }
+ if (m_frontRightMotor != null) {
+ motors++;
+ }
+ if (m_rearLeftMotor != null) {
+ motors++;
+ }
+ if (m_rearRightMotor != null) {
+ motors++;
+ }
+ return motors;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
new file mode 100644
index 0000000..0f384ee
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+@SuppressWarnings("JavadocMethod")
+public final class RobotState {
+ public static boolean isDisabled() {
+ return DriverStation.getInstance().isDisabled();
+ }
+
+ public static boolean isEnabled() {
+ return DriverStation.getInstance().isEnabled();
+ }
+
+ public static boolean isOperatorControl() {
+ return DriverStation.getInstance().isOperatorControl();
+ }
+
+ public static boolean isAutonomous() {
+ return DriverStation.getInstance().isAutonomous();
+ }
+
+ public static boolean isTest() {
+ return DriverStation.getInstance().isTest();
+ }
+
+ private RobotState() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
new file mode 100644
index 0000000..8f9ce0d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Mindsensors SD540 Speed Controller.
+ */
+public class SD540 extends PWMSpeedController {
+ /**
+ * Common initialization code called by all constructors.
+ *
+ * <p>Note that the SD540 uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the SD540 User Manual
+ * available from Mindsensors.
+ *
+ * <p>- 2.05ms = full "forward" - 1.55ms = the "high end" of the deadband range - 1.50ms = center
+ * of the deadband range (off) - 1.44ms = the "low end" of the deadband range - .94ms = full
+ * "reverse"
+ */
+ protected void initSD540() {
+ setBounds(2.05, 1.55, 1.50, 1.44, .94);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel());
+ setName("SD540", getChannel());
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public SD540(final int channel) {
+ super(channel);
+ initSD540();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
new file mode 100644
index 0000000..70b9d82
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -0,0 +1,804 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.IntBuffer;
+
+import edu.wpi.first.hal.AccumulatorResult;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SPIJNI;
+
+/**
+ * Represents a SPI bus port.
+ */
+@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.TooManyMethods"})
+public class SPI implements AutoCloseable {
+ public enum Port {
+ kOnboardCS0(0), kOnboardCS1(1), kOnboardCS2(2), kOnboardCS3(3), kMXP(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Port(int value) {
+ this.value = value;
+ }
+ }
+
+ private static int devices;
+
+ private int m_port;
+ private int m_msbFirst;
+ private int m_clockIdleHigh;
+ private int m_sampleOnTrailing;
+
+ /**
+ * Constructor.
+ *
+ * @param port the physical SPI port
+ */
+ public SPI(Port port) {
+ m_port = (byte) port.value;
+ devices++;
+
+ SPIJNI.spiInitialize(m_port);
+
+ HAL.report(tResourceType.kResourceType_SPI, devices);
+ }
+
+
+ @Deprecated
+ public void free() {
+ close();
+ }
+
+ @Override
+ public void close() {
+ if (m_accum != null) {
+ m_accum.close();
+ m_accum = null;
+ }
+ SPIJNI.spiClose(m_port);
+ }
+
+ /**
+ * Configure the rate of the generated clock signal. The default value is 500,000 Hz. The maximum
+ * value is 4,000,000 Hz.
+ *
+ * @param hz The clock rate in Hertz.
+ */
+ public final void setClockRate(int hz) {
+ SPIJNI.spiSetSpeed(m_port, hz);
+ }
+
+ /**
+ * Configure the order that bits are sent and received on the wire to be most significant bit
+ * first.
+ */
+ public final void setMSBFirst() {
+ m_msbFirst = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure the order that bits are sent and received on the wire to be least significant bit
+ * first.
+ */
+ public final void setLSBFirst() {
+ m_msbFirst = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure the clock output line to be active low. This is sometimes called clock polarity high
+ * or clock idle high.
+ */
+ public final void setClockActiveLow() {
+ m_clockIdleHigh = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure the clock output line to be active high. This is sometimes called clock polarity low
+ * or clock idle low.
+ */
+ public final void setClockActiveHigh() {
+ m_clockIdleHigh = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the leading edge and the data changes on the trailing
+ * edge.
+ */
+ public final void setSampleDataOnLeadingEdge() {
+ m_sampleOnTrailing = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the trailing edge and the data changes on the leading
+ * edge.
+ */
+ public final void setSampleDataOnTrailingEdge() {
+ m_sampleOnTrailing = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the falling edge and the data changes on the rising edge.
+ * Note this gets reversed is setClockActiveLow is set.
+ * @deprecated use {@link #setSampleDataOnTrailingEdge()} in most cases.
+ */
+ @Deprecated
+ public final void setSampleDataOnFalling() {
+ m_sampleOnTrailing = 1;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+ /**
+ * Configure that the data is stable on the rising edge and the data changes on the falling edge.
+ * Note this gets reversed is setClockActiveLow is set.
+ * @deprecated use {@link #setSampleDataOnLeadingEdge()} in most cases.
+ */
+ @Deprecated
+ public final void setSampleDataOnRising() {
+ m_sampleOnTrailing = 0;
+ SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ }
+
+
+
+ /**
+ * Configure the chip select line to be active high.
+ */
+ public final void setChipSelectActiveHigh() {
+ SPIJNI.spiSetChipSelectActiveHigh(m_port);
+ }
+
+ /**
+ * Configure the chip select line to be active low.
+ */
+ public final void setChipSelectActiveLow() {
+ SPIJNI.spiSetChipSelectActiveLow(m_port);
+ }
+
+ /**
+ * Write data to the slave device. Blocks until there is space in the output FIFO.
+ *
+ * <p>If not running in output only mode, also saves the data received on the MISO input during
+ * the transfer into the receive FIFO.
+ */
+ public int write(byte[] dataToSend, int size) {
+ if (dataToSend.length < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiWriteB(m_port, dataToSend, (byte) size);
+ }
+
+ /**
+ * Write data to the slave device. Blocks until there is space in the output FIFO.
+ *
+ * <p>If not running in output only mode, also saves the data received on the MISO input during
+ * the transfer into the receive FIFO.
+ *
+ * @param dataToSend The buffer containing the data to send.
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public int write(ByteBuffer dataToSend, int size) {
+ if (dataToSend.hasArray()) {
+ return write(dataToSend.array(), size);
+ }
+ if (!dataToSend.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (dataToSend.capacity() < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiWrite(m_port, dataToSend, (byte) size);
+ }
+
+ /**
+ * Read a word from the receive FIFO.
+ *
+ * <p>Waits for the current transfer to complete if the receive FIFO is empty.
+ *
+ * <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
+ *
+ * @param initiate If true, this function pushes "0" into the transmit buffer and initiates a
+ * transfer. If false, this function assumes that data is already in the receive
+ * FIFO from a previous write.
+ */
+ public int read(boolean initiate, byte[] dataReceived, int size) {
+ if (dataReceived.length < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiReadB(m_port, initiate, dataReceived, (byte) size);
+ }
+
+ /**
+ * Read a word from the receive FIFO.
+ *
+ * <p>Waits for the current transfer to complete if the receive FIFO is empty.
+ *
+ * <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
+ *
+ * @param initiate If true, this function pushes "0" into the transmit buffer and initiates
+ * a transfer. If false, this function assumes that data is already in the
+ * receive FIFO from a previous write.
+ * @param dataReceived The buffer to be filled with the received data.
+ * @param size The length of the transaction, in bytes
+ */
+ @SuppressWarnings("ByteBufferBackingArray")
+ public int read(boolean initiate, ByteBuffer dataReceived, int size) {
+ if (dataReceived.hasArray()) {
+ return read(initiate, dataReceived.array(), size);
+ }
+ if (!dataReceived.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (dataReceived.capacity() < size) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + size);
+ }
+ return SPIJNI.spiRead(m_port, initiate, dataReceived, (byte) size);
+ }
+
+ /**
+ * Perform a simultaneous read/write transaction with the device.
+ *
+ * @param dataToSend The data to be written out to the device
+ * @param dataReceived Buffer to receive data from the device
+ * @param size The length of the transaction, in bytes
+ */
+ public int transaction(byte[] dataToSend, byte[] dataReceived, int size) {
+ if (dataToSend.length < size) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
+ }
+ if (dataReceived.length < size) {
+ throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
+ }
+ return SPIJNI.spiTransactionB(m_port, dataToSend, dataReceived, (byte) size);
+ }
+
+ /**
+ * Perform a simultaneous read/write transaction with the device.
+ *
+ * @param dataToSend The data to be written out to the device.
+ * @param dataReceived Buffer to receive data from the device.
+ * @param size The length of the transaction, in bytes
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "ByteBufferBackingArray"})
+ public int transaction(ByteBuffer dataToSend, ByteBuffer dataReceived, int size) {
+ if (dataToSend.hasArray() && dataReceived.hasArray()) {
+ return transaction(dataToSend.array(), dataReceived.array(), size);
+ }
+ if (!dataToSend.isDirect()) {
+ throw new IllegalArgumentException("dataToSend must be a direct buffer");
+ }
+ if (dataToSend.capacity() < size) {
+ throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
+ }
+ if (!dataReceived.isDirect()) {
+ throw new IllegalArgumentException("dataReceived must be a direct buffer");
+ }
+ if (dataReceived.capacity() < size) {
+ throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
+ }
+ return SPIJNI.spiTransaction(m_port, dataToSend, dataReceived, (byte) size);
+ }
+
+ /**
+ * Initialize automatic SPI transfer engine.
+ *
+ * <p>Only a single engine is available, and use of it blocks use of all other
+ * chip select usage on the same physical SPI port while it is running.
+ *
+ * @param bufferSize buffer size in bytes
+ */
+ public void initAuto(int bufferSize) {
+ SPIJNI.spiInitAuto(m_port, bufferSize);
+ }
+
+ /**
+ * Frees the automatic SPI transfer engine.
+ */
+ public void freeAuto() {
+ SPIJNI.spiFreeAuto(m_port);
+ }
+
+ /**
+ * Set the data to be transmitted by the engine.
+ *
+ * <p>Up to 16 bytes are configurable, and may be followed by up to 127 zero
+ * bytes.
+ *
+ * @param dataToSend data to send (maximum 16 bytes)
+ * @param zeroSize number of zeros to send after the data
+ */
+ public void setAutoTransmitData(byte[] dataToSend, int zeroSize) {
+ SPIJNI.spiSetAutoTransmitData(m_port, dataToSend, zeroSize);
+ }
+
+ /**
+ * Start running the automatic SPI transfer engine at a periodic rate.
+ *
+ * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must
+ * be called before calling this function.
+ *
+ * @param period period between transfers, in seconds (us resolution)
+ */
+ public void startAutoRate(double period) {
+ SPIJNI.spiStartAutoRate(m_port, period);
+ }
+
+ /**
+ * Start running the automatic SPI transfer engine when a trigger occurs.
+ *
+ * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must
+ * be called before calling this function.
+ *
+ * @param source digital source for the trigger (may be an analog trigger)
+ * @param rising trigger on the rising edge
+ * @param falling trigger on the falling edge
+ */
+ public void startAutoTrigger(DigitalSource source, boolean rising, boolean falling) {
+ SPIJNI.spiStartAutoTrigger(m_port, source.getPortHandleForRouting(),
+ source.getAnalogTriggerTypeForRouting(), rising, falling);
+ }
+
+ /**
+ * Stop running the automatic SPI transfer engine.
+ */
+ public void stopAuto() {
+ SPIJNI.spiStopAuto(m_port);
+ }
+
+ /**
+ * Force the engine to make a single transfer.
+ */
+ public void forceAutoRead() {
+ SPIJNI.spiForceAutoRead(m_port);
+ }
+
+ /**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * <p>Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * <p>Each received data sequence consists of a timestamp followed by the
+ * received data bytes, one byte per word (in the least significant byte).
+ * The length of each received data sequence is the same as the combined
+ * size of the data and zeroSize set in setAutoTransmitData().
+ *
+ * <p>Blocks until numToRead words have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many words are available.
+ *
+ * @param buffer buffer where read words are stored
+ * @param numToRead number of words to read
+ * @param timeout timeout in seconds (ms resolution)
+ * @return Number of words remaining to be read
+ */
+ public int readAutoReceivedData(ByteBuffer buffer, int numToRead, double timeout) {
+ if (!buffer.isDirect()) {
+ throw new IllegalArgumentException("must be a direct buffer");
+ }
+ if (buffer.capacity() < numToRead * 4) {
+ throw new IllegalArgumentException("buffer is too small, must be at least "
+ + (numToRead * 4));
+ }
+ return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
+ }
+
+ /**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * <p>Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * <p>Each received data sequence consists of a timestamp followed by the
+ * received data bytes, one byte per word (in the least significant byte).
+ * The length of each received data sequence is the same as the combined
+ * size of the data and zeroSize set in setAutoTransmitData().
+ *
+ * <p>Blocks until numToRead words have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many words are available.
+ *
+ * @param buffer array where read words are stored
+ * @param numToRead number of words to read
+ * @param timeout timeout in seconds (ms resolution)
+ * @return Number of words remaining to be read
+ */
+ public int readAutoReceivedData(int[] buffer, int numToRead, double timeout) {
+ if (buffer.length < numToRead) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead);
+ }
+ return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
+ }
+
+ /**
+ * Get the number of bytes dropped by the automatic SPI transfer engine due
+ * to the receive buffer being full.
+ *
+ * @return Number of bytes dropped
+ */
+ public int getAutoDroppedCount() {
+ return SPIJNI.spiGetAutoDroppedCount(m_port);
+ }
+
+ private static final int kAccumulateDepth = 2048;
+
+ @SuppressWarnings("PMD.TooManyFields")
+ private static class Accumulator implements AutoCloseable {
+ Accumulator(int port, int xferSize, int validMask, int validValue, int dataShift,
+ int dataSize, boolean isSigned, boolean bigEndian) {
+ m_notifier = new Notifier(this::update);
+ m_buf = ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4)
+ .order(ByteOrder.nativeOrder());
+ m_intBuf = m_buf.asIntBuffer();
+ m_xferSize = xferSize + 1; // +1 for timestamp
+ m_validMask = validMask;
+ m_validValue = validValue;
+ m_dataShift = dataShift;
+ m_dataMax = 1 << dataSize;
+ m_dataMsbMask = 1 << (dataSize - 1);
+ m_isSigned = isSigned;
+ m_bigEndian = bigEndian;
+ m_port = port;
+ }
+
+ @Override
+ public void close() {
+ m_notifier.close();
+ }
+
+ final Notifier m_notifier;
+ final ByteBuffer m_buf;
+ final IntBuffer m_intBuf;
+ final Object m_mutex = new Object();
+
+ long m_value;
+ int m_count;
+ int m_lastValue;
+ long m_lastTimestamp;
+ double m_integratedValue;
+
+ int m_center;
+ int m_deadband;
+ double m_integratedCenter;
+
+ final int m_validMask;
+ final int m_validValue;
+ final int m_dataMax; // one more than max data value
+ final int m_dataMsbMask; // data field MSB mask (for signed)
+ final int m_dataShift; // data field shift right amount, in bits
+ final int m_xferSize; // SPI transfer size, in bytes
+ final boolean m_isSigned; // is data field signed?
+ final boolean m_bigEndian; // is response big endian?
+ final int m_port;
+
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ void update() {
+ synchronized (m_mutex) {
+ boolean done = false;
+ while (!done) {
+ done = true;
+
+ // get amount of data available
+ int numToRead = SPIJNI.spiReadAutoReceivedData(m_port, m_buf, 0, 0);
+
+ // only get whole responses
+ numToRead -= numToRead % m_xferSize;
+ if (numToRead > m_xferSize * kAccumulateDepth) {
+ numToRead = m_xferSize * kAccumulateDepth;
+ done = false;
+ }
+ if (numToRead == 0) {
+ return; // no samples
+ }
+
+ // read buffered data
+ SPIJNI.spiReadAutoReceivedData(m_port, m_buf, numToRead, 0);
+
+ // loop over all responses
+ for (int off = 0; off < numToRead; off += m_xferSize) {
+ // get timestamp from first word
+ long timestamp = m_intBuf.get(off) & 0xffffffffL;
+
+ // convert from bytes
+ int resp = 0;
+ if (m_bigEndian) {
+ for (int i = 1; i < m_xferSize; ++i) {
+ resp <<= 8;
+ resp |= m_intBuf.get(off + i) & 0xff;
+ }
+ } else {
+ for (int i = m_xferSize - 1; i >= 1; --i) {
+ resp <<= 8;
+ resp |= m_intBuf.get(off + i) & 0xff;
+ }
+ }
+
+ // process response
+ if ((resp & m_validMask) == m_validValue) {
+ // valid sensor data; extract data field
+ int data = resp >> m_dataShift;
+ data &= m_dataMax - 1;
+ // 2s complement conversion if signed MSB is set
+ if (m_isSigned && (data & m_dataMsbMask) != 0) {
+ data -= m_dataMax;
+ }
+ // center offset
+ int dataNoCenter = data;
+ data -= m_center;
+ // only accumulate if outside deadband
+ if (data < -m_deadband || data > m_deadband) {
+ m_value += data;
+ if (m_count != 0) {
+ // timestamps use the 1us FPGA clock; also handle rollover
+ if (timestamp >= m_lastTimestamp) {
+ m_integratedValue += dataNoCenter * (timestamp - m_lastTimestamp)
+ * 1e-6 - m_integratedCenter;
+ } else {
+ m_integratedValue += dataNoCenter * ((1L << 32) - m_lastTimestamp + timestamp)
+ * 1e-6 - m_integratedCenter;
+ }
+ }
+ }
+ ++m_count;
+ m_lastValue = data;
+ } else {
+ // no data from the sensor; just clear the last value
+ m_lastValue = 0;
+ }
+ m_lastTimestamp = timestamp;
+ }
+ }
+ }
+ }
+ }
+
+ private Accumulator m_accum;
+
+ /**
+ * Initialize the accumulator.
+ *
+ * @param period Time between reads
+ * @param cmd SPI command to send to request data
+ * @param xferSize SPI transfer size, in bytes
+ * @param validMask Mask to apply to received data for validity checking
+ * @param validValue After validMask is applied, required matching value for validity checking
+ * @param dataShift Bit shift to apply to received data to get actual data value
+ * @param dataSize Size (in bits) of data field
+ * @param isSigned Is data field signed?
+ * @param bigEndian Is device big endian?
+ */
+ public void initAccumulator(double period, int cmd, int xferSize,
+ int validMask, int validValue,
+ int dataShift, int dataSize,
+ boolean isSigned, boolean bigEndian) {
+ initAuto(xferSize * 2048);
+ byte[] cmdBytes = new byte[] {0, 0, 0, 0};
+ if (bigEndian) {
+ for (int i = xferSize - 1; i >= 0; --i) {
+ cmdBytes[i] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ }
+ } else {
+ cmdBytes[0] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ cmdBytes[1] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ cmdBytes[2] = (byte) (cmd & 0xff);
+ cmd >>= 8;
+ cmdBytes[3] = (byte) (cmd & 0xff);
+ }
+ setAutoTransmitData(cmdBytes, xferSize - 4);
+ startAutoRate(period);
+
+ m_accum = new Accumulator(m_port, xferSize, validMask, validValue, dataShift, dataSize,
+ isSigned, bigEndian);
+ m_accum.m_notifier.startPeriodic(period * 1024);
+ }
+
+ /**
+ * Frees the accumulator.
+ */
+ public void freeAccumulator() {
+ if (m_accum != null) {
+ m_accum.close();
+ m_accum = null;
+ }
+ freeAuto();
+ }
+
+ /**
+ * Resets the accumulator to zero.
+ */
+ public void resetAccumulator() {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_value = 0;
+ m_accum.m_count = 0;
+ m_accum.m_lastValue = 0;
+ m_accum.m_lastTimestamp = 0;
+ m_accum.m_integratedValue = 0;
+ }
+ }
+
+ /**
+ * Set the center value of the accumulator.
+ *
+ * <p>The center value is subtracted from each value before it is added to the accumulator. This
+ * is used for the center value of devices like gyros and accelerometers to make integration work
+ * and to take the device offset into account when integrating.
+ */
+ public void setAccumulatorCenter(int center) {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_center = center;
+ }
+ }
+
+ /**
+ * Set the accumulator's deadband.
+ */
+ public void setAccumulatorDeadband(int deadband) {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_deadband = deadband;
+ }
+ }
+
+ /**
+ * Read the last value read by the accumulator engine.
+ */
+ public int getAccumulatorLastValue() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_lastValue;
+ }
+ }
+
+ /**
+ * Read the accumulated value.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+ public long getAccumulatorValue() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_value;
+ }
+ }
+
+ /**
+ * Read the number of accumulated values.
+ *
+ * <p>Read the count of the accumulated values since the accumulator was last Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+ public int getAccumulatorCount() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_count;
+ }
+ }
+
+ /**
+ * Read the average of the accumulated value.
+ *
+ * @return The accumulated average value (value / count).
+ */
+ public double getAccumulatorAverage() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ if (m_accum.m_count == 0) {
+ return 0.0;
+ }
+ return ((double) m_accum.m_value) / m_accum.m_count;
+ }
+ }
+
+ /**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * <p>This function reads the value and count atomically. This can be used for averaging.
+ *
+ * @param result AccumulatorResult object to store the results in.
+ */
+ public void getAccumulatorOutput(AccumulatorResult result) {
+ if (result == null) {
+ throw new IllegalArgumentException("Null parameter `result'");
+ }
+ if (m_accum == null) {
+ result.value = 0;
+ result.count = 0;
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ result.value = m_accum.m_value;
+ result.count = m_accum.m_count;
+ }
+ }
+
+ /**
+ * Set the center value of the accumulator integrator.
+ *
+ * <p>The center value is subtracted from each value*dt before it is added to the
+ * integrated value. This is used for the center value of devices like gyros
+ * and accelerometers to take the device offset into account when integrating.
+ */
+ public void setAccumulatorIntegratedCenter(double center) {
+ if (m_accum == null) {
+ return;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.m_integratedCenter = center;
+ }
+ }
+
+ /**
+ * Read the integrated value. This is the sum of (each value * time between
+ * values).
+ *
+ * @return The integrated value accumulated since the last Reset().
+ */
+ public double getAccumulatorIntegratedValue() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ return m_accum.m_integratedValue;
+ }
+ }
+
+ /**
+ * Read the average of the integrated value. This is the sum of (each value
+ * times the time between values), divided by the count.
+ *
+ * @return The average of the integrated value accumulated since the last
+ * Reset().
+ */
+ public double getAccumulatorIntegratedAverage() {
+ if (m_accum == null) {
+ return 0;
+ }
+ synchronized (m_accum.m_mutex) {
+ m_accum.update();
+ if (m_accum.m_count <= 1) {
+ return 0.0;
+ }
+ // count-1 due to not integrating the first value received
+ return m_accum.m_integratedValue / (m_accum.m_count - 1);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SampleRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SampleRobot.java
new file mode 100644
index 0000000..6c13013
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SampleRobot.java
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+
+/**
+ * A simple robot base class that knows the standard FRC competition states (disabled, autonomous,
+ * or operator controlled).
+ *
+ * <p>You can build a simple robot program off of this by overriding the robotinit(), disabled(),
+ * autonomous() and operatorControl() methods. The startCompetition() method will calls these
+ * methods (sometimes repeatedly). depending on the state of the competition.
+ *
+ * <p>Alternatively you can override the robotMain() method and manage all aspects of the robot
+ * yourself.
+ *
+ * @deprecated WARNING: While it may look like a good choice to use for your code if you're
+ * inexperienced, don't. Unless you know what you are doing, complex code will
+ * be much more difficult under this system. Use TimedRobot or Command-Based
+ * instead.
+ */
+@Deprecated
+public class SampleRobot extends RobotBase {
+ private boolean m_robotMainOverridden = true;
+
+ /**
+ * Create a new SampleRobot.
+ */
+ public SampleRobot() {
+ HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Simple);
+ }
+
+ /**
+ * Robot-wide initialization code should go here.
+ *
+ * <p>Users should override this method for default Robot-wide initialization which will be called
+ * when the robot is first powered on. It will be called exactly one time.
+ *
+ * <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
+ * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
+ * never indicate that the code is ready, causing the robot to be bypassed in a match.
+ */
+ protected void robotInit() {
+ System.out.println("Default robotInit() method running, consider providing your own");
+ }
+
+ /**
+ * Disabled should go here. Users should overload this method to run code that should run while
+ * the field is disabled.
+ *
+ * <p>Called once each time the robot enters the disabled state.
+ */
+ protected void disabled() {
+ System.out.println("Default disabled() method running, consider providing your own");
+ }
+
+ /**
+ * Autonomous should go here. Users should add autonomous code to this method that should run
+ * while the field is in the autonomous period.
+ *
+ * <p>Called once each time the robot enters the autonomous state.
+ */
+ public void autonomous() {
+ System.out.println("Default autonomous() method running, consider providing your own");
+ }
+
+ /**
+ * Operator control (tele-operated) code should go here. Users should add Operator Control code to
+ * this method that should run while the field is in the Operator Control (tele-operated) period.
+ *
+ * <p>Called once each time the robot enters the operator-controlled state.
+ */
+ public void operatorControl() {
+ System.out.println("Default operatorControl() method running, consider providing your own");
+ }
+
+ /**
+ * Test code should go here. Users should add test code to this method that should run while the
+ * robot is in test mode.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void test() {
+ System.out.println("Default test() method running, consider providing your own");
+ }
+
+ /**
+ * Robot main program for free-form programs.
+ *
+ * <p>This should be overridden by user subclasses if the intent is to not use the autonomous()
+ * and operatorControl() methods. In that case, the program is responsible for sensing when to run
+ * the autonomous and operator control functions in their program.
+ *
+ * <p>This method will be called immediately after the constructor is called. If it has not been
+ * overridden by a user subclass (i.e. the default version runs), then the robotInit(),
+ * disabled(), autonomous() and operatorControl() methods will be called.
+ */
+ public void robotMain() {
+ m_robotMainOverridden = false;
+ }
+
+ /**
+ * Start a competition. This code tracks the order of the field starting to ensure that everything
+ * happens in the right order. Repeatedly run the correct method, either Autonomous or
+ * OperatorControl when the robot is enabled. After running the correct method, wait for some
+ * state to change, either the other mode starts or the robot is disabled. Then go back and wait
+ * for the robot to be enabled again.
+ */
+ @SuppressWarnings("PMD.CyclomaticComplexity")
+ @Override
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ robotMain();
+
+ if (!m_robotMainOverridden) {
+ while (true) {
+ if (isDisabled()) {
+ m_ds.InDisabled(true);
+ disabled();
+ m_ds.InDisabled(false);
+ while (isDisabled()) {
+ Timer.delay(0.01);
+ }
+ } else if (isAutonomous()) {
+ m_ds.InAutonomous(true);
+ autonomous();
+ m_ds.InAutonomous(false);
+ while (isAutonomous() && !isDisabled()) {
+ Timer.delay(0.01);
+ }
+ } else if (isTest()) {
+ LiveWindow.setEnabled(true);
+ Shuffleboard.enableActuatorWidgets();
+ m_ds.InTest(true);
+ test();
+ m_ds.InTest(false);
+ while (isTest() && isEnabled()) {
+ Timer.delay(0.01);
+ }
+ LiveWindow.setEnabled(false);
+ Shuffleboard.disableActuatorWidgets();
+ } else {
+ m_ds.InOperatorControl(true);
+ operatorControl();
+ m_ds.InOperatorControl(false);
+ while (isOperatorControl() && !isDisabled()) {
+ Timer.delay(0.01);
+ }
+ }
+ } /* while loop */
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
new file mode 100644
index 0000000..76eb03c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+
+/**
+ * The base interface for objects that can be sent over the network through network tables.
+ */
+public interface Sendable {
+ /**
+ * Gets the name of this {@link Sendable} object.
+ *
+ * @return Name
+ */
+ String getName();
+
+ /**
+ * Sets the name of this {@link Sendable} object.
+ *
+ * @param name name
+ */
+ void setName(String name);
+
+ /**
+ * Sets both the subsystem name and device name of this {@link Sendable} object.
+ *
+ * @param subsystem subsystem name
+ * @param name device name
+ */
+ default void setName(String subsystem, String name) {
+ setSubsystem(subsystem);
+ setName(name);
+ }
+
+ /**
+ * Gets the subsystem name of this {@link Sendable} object.
+ *
+ * @return Subsystem name
+ */
+ String getSubsystem();
+
+ /**
+ * Sets the subsystem name of this {@link Sendable} object.
+ *
+ * @param subsystem subsystem name
+ */
+ void setSubsystem(String subsystem);
+
+ /**
+ * Initializes this {@link Sendable} object.
+ *
+ * @param builder sendable builder
+ */
+ void initSendable(SendableBuilder builder);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
new file mode 100644
index 0000000..b663ed5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * Base class for all sensors. Stores most recent status information as well as containing utility
+ * functions for checking channels and error processing.
+ */
+public abstract class SendableBase implements Sendable, AutoCloseable {
+ private String m_name = "";
+ private String m_subsystem = "Ungrouped";
+
+ /**
+ * Creates an instance of the sensor base.
+ */
+ public SendableBase() {
+ this(true);
+ }
+
+ /**
+ * Creates an instance of the sensor base.
+ *
+ * @param addLiveWindow if true, add this Sendable to LiveWindow
+ */
+ public SendableBase(boolean addLiveWindow) {
+ if (addLiveWindow) {
+ LiveWindow.add(this);
+ }
+ }
+
+ @Deprecated
+ public void free() {
+ close();
+ }
+
+ @Override
+ public void close() {
+ LiveWindow.remove(this);
+ }
+
+ @Override
+ public final synchronized String getName() {
+ return m_name;
+ }
+
+ @Override
+ public final synchronized void setName(String name) {
+ m_name = name;
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ */
+ protected final void setName(String moduleType, int channel) {
+ setName(moduleType + "[" + channel + "]");
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ */
+ protected final void setName(String moduleType, int moduleNumber, int channel) {
+ setName(moduleType + "[" + moduleNumber + "," + channel + "]");
+ }
+
+ @Override
+ public final synchronized String getSubsystem() {
+ return m_subsystem;
+ }
+
+ @Override
+ public final synchronized void setSubsystem(String subsystem) {
+ m_subsystem = subsystem;
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ protected final void addChild(Object child) {
+ LiveWindow.addChild(this, child);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableImpl.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableImpl.java
new file mode 100644
index 0000000..0fa2e74
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableImpl.java
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * The base interface for objects that can be sent over the network through network tables.
+ */
+public class SendableImpl implements Sendable, AutoCloseable {
+ private String m_name = "";
+ private String m_subsystem = "Ungrouped";
+
+ /**
+ * Creates an instance of the sensor base.
+ */
+ public SendableImpl() {
+ this(true);
+ }
+
+ /**
+ * Creates an instance of the sensor base.
+ *
+ * @param addLiveWindow if true, add this Sendable to LiveWindow
+ */
+ public SendableImpl(boolean addLiveWindow) {
+ if (addLiveWindow) {
+ LiveWindow.add(this);
+ }
+ }
+
+ @Deprecated
+ public void free() {
+ close();
+ }
+
+ @Override
+ public void close() {
+ LiveWindow.remove(this);
+ }
+
+ @Override
+ public synchronized String getName() {
+ return m_name;
+ }
+
+ @Override
+ public synchronized void setName(String name) {
+ m_name = name;
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ */
+ public void setName(String moduleType, int channel) {
+ setName(moduleType + "[" + channel + "]");
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ */
+ public void setName(String moduleType, int moduleNumber, int channel) {
+ setName(moduleType + "[" + moduleNumber + "," + channel + "]");
+ }
+
+ @Override
+ public synchronized String getSubsystem() {
+ return m_subsystem;
+ }
+
+ @Override
+ public synchronized void setSubsystem(String subsystem) {
+ m_subsystem = subsystem;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ public void addChild(Object child) {
+ LiveWindow.addChild(this, child);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
new file mode 100644
index 0000000..4f2b8b5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
@@ -0,0 +1,236 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.ConstantsJNI;
+import edu.wpi.first.hal.DIOJNI;
+import edu.wpi.first.hal.PDPJNI;
+import edu.wpi.first.hal.PWMJNI;
+import edu.wpi.first.hal.PortsJNI;
+import edu.wpi.first.hal.RelayJNI;
+import edu.wpi.first.hal.SolenoidJNI;
+
+/**
+ * Stores most recent status information as well as containing utility functions for checking
+ * channels and error processing.
+ */
+public final class SensorUtil {
+ /**
+ * Ticks per microsecond.
+ */
+ public static final int kSystemClockTicksPerMicrosecond
+ = ConstantsJNI.getSystemClockTicksPerMicrosecond();
+
+ /**
+ * Number of digital channels per roboRIO.
+ */
+ public static final int kDigitalChannels = PortsJNI.getNumDigitalChannels();
+
+ /**
+ * Number of analog input channels per roboRIO.
+ */
+ public static final int kAnalogInputChannels = PortsJNI.getNumAnalogInputs();
+
+ /**
+ * Number of analog output channels per roboRIO.
+ */
+ public static final int kAnalogOutputChannels = PortsJNI.getNumAnalogOutputs();
+
+ /**
+ * Number of solenoid channels per module.
+ */
+ public static final int kSolenoidChannels = PortsJNI.getNumSolenoidChannels();
+
+ /**
+ * Number of PWM channels per roboRIO.
+ */
+ public static final int kPwmChannels = PortsJNI.getNumPWMChannels();
+
+ /**
+ * Number of relay channels per roboRIO.
+ */
+ public static final int kRelayChannels = PortsJNI.getNumRelayHeaders();
+
+ /**
+ * Number of power distribution channels per PDP.
+ */
+ public static final int kPDPChannels = PortsJNI.getNumPDPChannels();
+
+ /**
+ * Number of power distribution modules per PDP.
+ */
+ public static final int kPDPModules = PortsJNI.getNumPDPModules();
+
+ /**
+ * Number of PCM Modules.
+ */
+ public static final int kPCMModules = PortsJNI.getNumPCMModules();
+
+ /**
+ * Verify that the solenoid module is correct.
+ *
+ * @param moduleNumber The solenoid module module number to check.
+ */
+ public static void checkSolenoidModule(final int moduleNumber) {
+ if (!SolenoidJNI.checkSolenoidModule(moduleNumber)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested solenoid module is out of range. Minimum: 0, Maximum: ")
+ .append(kPCMModules)
+ .append(", Requested: ")
+ .append(moduleNumber);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the digital channel number is valid. Verify that the channel number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkDigitalChannel(final int channel) {
+ if (!DIOJNI.checkDIOChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested DIO channel is out of range. Minimum: 0, Maximum: ")
+ .append(kDigitalChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the digital channel number is valid. Verify that the channel number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkRelayChannel(final int channel) {
+ if (!RelayJNI.checkRelayChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested relay channel is out of range. Minimum: 0, Maximum: ")
+ .append(kRelayChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the digital channel number is valid. Verify that the channel number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkPWMChannel(final int channel) {
+ if (!PWMJNI.checkPWMChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested PWM channel is out of range. Minimum: 0, Maximum: ")
+ .append(kPwmChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the analog input number is value. Verify that the analog input number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkAnalogInputChannel(final int channel) {
+ if (!AnalogJNI.checkAnalogInputChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested analog input channel is out of range. Minimum: 0, Maximum: ")
+ .append(kAnalogInputChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Check that the analog input number is value. Verify that the analog input number is one of the
+ * legal channel numbers. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkAnalogOutputChannel(final int channel) {
+ if (!AnalogJNI.checkAnalogOutputChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested analog output channel is out of range. Minimum: 0, Maximum: ")
+ .append(kAnalogOutputChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Verify that the solenoid channel number is within limits. Channel numbers are 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkSolenoidChannel(final int channel) {
+ if (!SolenoidJNI.checkSolenoidChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested solenoid channel is out of range. Minimum: 0, Maximum: ")
+ .append(kSolenoidChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Verify that the power distribution channel number is within limits. Channel numbers are
+ * 0-based.
+ *
+ * @param channel The channel number to check.
+ */
+ public static void checkPDPChannel(final int channel) {
+ if (!PDPJNI.checkPDPChannel(channel)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested PDP channel is out of range. Minimum: 0, Maximum: ")
+ .append(kPDPChannels)
+ .append(", Requested: ")
+ .append(channel);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Verify that the PDP module number is within limits. module numbers are 0-based.
+ *
+ * @param module The module number to check.
+ */
+ public static void checkPDPModule(final int module) {
+ if (!PDPJNI.checkPDPModule(module)) {
+ StringBuilder buf = new StringBuilder();
+ buf.append("Requested PDP module is out of range. Minimum: 0, Maximum: ")
+ .append(kPDPModules)
+ .append(", Requested: ")
+ .append(module);
+ throw new IndexOutOfBoundsException(buf.toString());
+ }
+ }
+
+ /**
+ * Get the number of the default solenoid module.
+ *
+ * @return The number of the default solenoid module.
+ */
+ public static int getDefaultSolenoidModule() {
+ return 0;
+ }
+
+ private SensorUtil() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
new file mode 100644
index 0000000..019a490
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -0,0 +1,394 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.UnsupportedEncodingException;
+import java.nio.charset.StandardCharsets;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SerialPortJNI;
+
+/**
+ * Driver for the RS-232 serial port on the roboRIO.
+ *
+ * <p>The current implementation uses the VISA formatted I/O mode. This means that all traffic goes
+ * through the formatted buffers. This allows the intermingled use of print(), readString(), and the
+ * raw buffer accessors read() and write().
+ *
+ * <p>More information can be found in the NI-VISA User Manual here: http://www.ni
+ * .com/pdf/manuals/370423a.pdf and the NI-VISA Programmer's Reference Manual here:
+ * http://www.ni.com/pdf/manuals/370132c.pdf
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class SerialPort implements AutoCloseable {
+ private byte m_port;
+
+ public enum Port {
+ kOnboard(0), kMXP(1), kUSB(2), kUSB1(2), kUSB2(3);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Port(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents the parity to use for serial communications.
+ */
+ public enum Parity {
+ kNone(0), kOdd(1), kEven(2), kMark(3), kSpace(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ Parity(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents the number of stop bits to use for Serial Communication.
+ */
+ public enum StopBits {
+ kOne(10), kOnePointFive(15), kTwo(20);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ StopBits(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents what type of flow control to use for serial communication.
+ */
+ public enum FlowControl {
+ kNone(0), kXonXoff(1), kRtsCts(2), kDtsDsr(4);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ FlowControl(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Represents which type of buffer mode to use when writing to a serial m_port.
+ */
+ public enum WriteBufferMode {
+ kFlushOnAccess(1), kFlushWhenFull(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ WriteBufferMode(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Create an instance of a Serial Port class.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param port The Serial port to use
+ * @param portName The direct portName to use
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ * @param stopBits The number of stop bits to use as defined by the enum StopBits.
+ * @deprecated Will be removed for 2019
+ */
+ @Deprecated
+ public SerialPort(final int baudRate, String portName, Port port, final int dataBits,
+ Parity parity, StopBits stopBits) {
+ m_port = (byte) port.value;
+
+ SerialPortJNI.serialInitializePortDirect(m_port, portName);
+ SerialPortJNI.serialSetBaudRate(m_port, baudRate);
+ SerialPortJNI.serialSetDataBits(m_port, (byte) dataBits);
+ SerialPortJNI.serialSetParity(m_port, (byte) parity.value);
+ SerialPortJNI.serialSetStopBits(m_port, (byte) stopBits.value);
+
+ // Set the default read buffer size to 1 to return bytes immediately
+ setReadBufferSize(1);
+
+ // Set the default timeout to 5 seconds.
+ setTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+
+ disableTermination();
+
+ HAL.report(tResourceType.kResourceType_SerialPort, 0);
+ }
+
+ /**
+ * Create an instance of a Serial Port class.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param port The Serial port to use
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ * @param stopBits The number of stop bits to use as defined by the enum StopBits.
+ */
+ public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity,
+ StopBits stopBits) {
+ m_port = (byte) port.value;
+
+ SerialPortJNI.serialInitializePort(m_port);
+ SerialPortJNI.serialSetBaudRate(m_port, baudRate);
+ SerialPortJNI.serialSetDataBits(m_port, (byte) dataBits);
+ SerialPortJNI.serialSetParity(m_port, (byte) parity.value);
+ SerialPortJNI.serialSetStopBits(m_port, (byte) stopBits.value);
+
+ // Set the default read buffer size to 1 to return bytes immediately
+ setReadBufferSize(1);
+
+ // Set the default timeout to 5 seconds.
+ setTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+
+ disableTermination();
+
+ HAL.report(tResourceType.kResourceType_SerialPort, 0);
+ }
+
+ /**
+ * Create an instance of a Serial Port class. Defaults to one stop bit.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ */
+ public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity) {
+ this(baudRate, port, dataBits, parity, StopBits.kOne);
+ }
+
+ /**
+ * Create an instance of a Serial Port class. Defaults to no parity and one stop bit.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+ */
+ public SerialPort(final int baudRate, Port port, final int dataBits) {
+ this(baudRate, port, dataBits, Parity.kNone, StopBits.kOne);
+ }
+
+ /**
+ * Create an instance of a Serial Port class. Defaults to 8 databits, no parity, and one stop
+ * bit.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ */
+ public SerialPort(final int baudRate, Port port) {
+ this(baudRate, port, 8, Parity.kNone, StopBits.kOne);
+ }
+
+ @Override
+ public void close() {
+ SerialPortJNI.serialClose(m_port);
+ }
+
+ /**
+ * Set the type of flow control to enable on this port.
+ *
+ * <p>By default, flow control is disabled.
+ *
+ * @param flowControl the FlowControl m_value to use
+ */
+ public void setFlowControl(FlowControl flowControl) {
+ SerialPortJNI.serialSetFlowControl(m_port, (byte) flowControl.value);
+ }
+
+ /**
+ * Enable termination and specify the termination character.
+ *
+ * <p>Termination is currently only implemented for receive. When the the terminator is received,
+ * the read() or readString() will return fewer bytes than requested, stopping after the
+ * terminator.
+ *
+ * @param terminator The character to use for termination.
+ */
+ public void enableTermination(char terminator) {
+ SerialPortJNI.serialEnableTermination(m_port, terminator);
+ }
+
+ /**
+ * Enable termination with the default terminator '\n'
+ *
+ * <p>Termination is currently only implemented for receive. When the the terminator is received,
+ * the read() or readString() will return fewer bytes than requested, stopping after the
+ * terminator.
+ *
+ * <p>The default terminator is '\n'
+ */
+ public void enableTermination() {
+ enableTermination('\n');
+ }
+
+ /**
+ * Disable termination behavior.
+ */
+ public void disableTermination() {
+ SerialPortJNI.serialDisableTermination(m_port);
+ }
+
+ /**
+ * Get the number of bytes currently available to read from the serial port.
+ *
+ * @return The number of bytes available to read.
+ */
+ public int getBytesReceived() {
+ return SerialPortJNI.serialGetBytesReceived(m_port);
+ }
+
+ /**
+ * Read a string out of the buffer. Reads the entire contents of the buffer
+ *
+ * @return The read string
+ */
+ public String readString() {
+ return readString(getBytesReceived());
+ }
+
+ /**
+ * Read a string out of the buffer. Reads the entire contents of the buffer
+ *
+ * @param count the number of characters to read into the string
+ * @return The read string
+ */
+ public String readString(int count) {
+ byte[] out = read(count);
+ try {
+ return new String(out, 0, out.length, "US-ASCII");
+ } catch (UnsupportedEncodingException ex) {
+ ex.printStackTrace();
+ return "";
+ }
+ }
+
+ /**
+ * Read raw bytes out of the buffer.
+ *
+ * @param count The maximum number of bytes to read.
+ * @return An array of the read bytes
+ */
+ public byte[] read(final int count) {
+ byte[] dataReceivedBuffer = new byte[count];
+ int gotten = SerialPortJNI.serialRead(m_port, dataReceivedBuffer, count);
+ if (gotten == count) {
+ return dataReceivedBuffer;
+ }
+ byte[] retVal = new byte[gotten];
+ System.arraycopy(dataReceivedBuffer, 0, retVal, 0, gotten);
+ return retVal;
+ }
+
+ /**
+ * Write raw bytes to the serial port.
+ *
+ * @param buffer The buffer of bytes to write.
+ * @param count The maximum number of bytes to write.
+ * @return The number of bytes actually written into the port.
+ */
+ public int write(byte[] buffer, int count) {
+ if (buffer.length < count) {
+ throw new IllegalArgumentException("buffer is too small, must be at least " + count);
+ }
+ return SerialPortJNI.serialWrite(m_port, buffer, count);
+ }
+
+ /**
+ * Write a string to the serial port.
+ *
+ * @param data The string to write to the serial port.
+ * @return The number of bytes actually written into the port.
+ */
+ public int writeString(String data) {
+ return write(data.getBytes(StandardCharsets.UTF_8), data.length());
+ }
+
+ /**
+ * Configure the timeout of the serial m_port.
+ *
+ * <p>This defines the timeout for transactions with the hardware. It will affect reads if less
+ * bytes are available than the read buffer size (defaults to 1) and very large writes.
+ *
+ * @param timeout The number of seconds to to wait for I/O.
+ */
+ public void setTimeout(double timeout) {
+ SerialPortJNI.serialSetTimeout(m_port, timeout);
+ }
+
+ /**
+ * Specify the size of the input buffer.
+ *
+ * <p>Specify the amount of data that can be stored before data from the device is returned to
+ * Read. If you want data that is received to be returned immediately, set this to 1.
+ *
+ * <p>It the buffer is not filled before the read timeout expires, all data that has been received
+ * so far will be returned.
+ *
+ * @param size The read buffer size.
+ */
+ public void setReadBufferSize(int size) {
+ SerialPortJNI.serialSetReadBufferSize(m_port, size);
+ }
+
+ /**
+ * Specify the size of the output buffer.
+ *
+ * <p>Specify the amount of data that can be stored before being transmitted to the device.
+ *
+ * @param size The write buffer size.
+ */
+ public void setWriteBufferSize(int size) {
+ SerialPortJNI.serialSetWriteBufferSize(m_port, size);
+ }
+
+ /**
+ * Specify the flushing behavior of the output buffer.
+ *
+ * <p>When set to kFlushOnAccess, data is synchronously written to the serial port after each
+ * call to either print() or write().
+ *
+ * <p>When set to kFlushWhenFull, data will only be written to the serial port when the buffer
+ * is full or when flush() is called.
+ *
+ * @param mode The write buffer mode.
+ */
+ public void setWriteBufferMode(WriteBufferMode mode) {
+ SerialPortJNI.serialSetWriteMode(m_port, (byte) mode.value);
+ }
+
+ /**
+ * Force the output buffer to be written to the port.
+ *
+ * <p>This is used when setWriteBufferMode() is set to kFlushWhenFull to force a flush before the
+ * buffer is full.
+ */
+ public void flush() {
+ SerialPortJNI.serialFlush(m_port);
+ }
+
+ /**
+ * Reset the serial port driver to a known state.
+ *
+ * <p>Empty the transmit and receive buffers in the device and formatted I/O.
+ */
+ public void reset() {
+ SerialPortJNI.serialClear(m_port);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
new file mode 100644
index 0000000..1abe6bb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Standard hobby style servo.
+ *
+ * <p>The range parameters default to the appropriate values for the Hitec HS-322HD servo provided
+ * in the FIRST Kit of Parts in 2008.
+ */
+public class Servo extends PWM {
+ private static final double kMaxServoAngle = 180.0;
+ private static final double kMinServoAngle = 0.0;
+
+ protected static final double kDefaultMaxServoPWM = 2.4;
+ protected static final double kDefaultMinServoPWM = .6;
+
+ /**
+ * Constructor.<br>
+ *
+ * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br> By default
+ * {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
+ *
+ * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Servo(final int channel) {
+ super(channel);
+ setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
+ setPeriodMultiplier(PeriodMultiplier.k4X);
+
+ HAL.report(tResourceType.kResourceType_Servo, getChannel());
+ setName("Servo", getChannel());
+ }
+
+
+ /**
+ * Set the servo position.
+ *
+ * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
+ *
+ * @param value Position from 0.0 to 1.0.
+ */
+ public void set(double value) {
+ setPosition(value);
+ }
+
+ /**
+ * Get the servo position.
+ *
+ * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
+ *
+ * @return Position from 0.0 to 1.0.
+ */
+ public double get() {
+ return getPosition();
+ }
+
+ /**
+ * Set the servo angle.
+ *
+ * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
+ * test).
+ *
+ * <p>Servo angles that are out of the supported range of the servo simply "saturate" in that
+ * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of
+ * less than X result in an angle of X being set and angles of more than Y degrees result in an
+ * angle of Y being set.
+ *
+ * @param degrees The angle in degrees to set the servo.
+ */
+ public void setAngle(double degrees) {
+ if (degrees < kMinServoAngle) {
+ degrees = kMinServoAngle;
+ } else if (degrees > kMaxServoAngle) {
+ degrees = kMaxServoAngle;
+ }
+
+ setPosition(((degrees - kMinServoAngle)) / getServoAngleRange());
+ }
+
+ /**
+ * Get the servo angle.
+ *
+ * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
+ * test).
+ *
+ * @return The angle in degrees to which the servo is set.
+ */
+ public double getAngle() {
+ return getPosition() * getServoAngleRange() + kMinServoAngle;
+ }
+
+ private double getServoAngleRange() {
+ return kMaxServoAngle - kMinServoAngle;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Servo");
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
new file mode 100644
index 0000000..014ec10
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SolenoidJNI;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Solenoid class for running high voltage Digital Output on the PCM.
+ *
+ * <p>The Solenoid class is typically used for pneumatic solenoids, but could be used for any
+ * device within the current spec of the PCM.
+ */
+public class Solenoid extends SolenoidBase {
+ private final int m_channel; // The channel to control.
+ private int m_solenoidHandle;
+
+ /**
+ * Constructor using the default PCM ID (defaults to 0).
+ *
+ * @param channel The channel on the PCM to control (0..7).
+ */
+ public Solenoid(final int channel) {
+ this(SensorUtil.getDefaultSolenoidModule(), channel);
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The CAN ID of the PCM the solenoid is attached to.
+ * @param channel The channel on the PCM to control (0..7).
+ */
+ public Solenoid(final int moduleNumber, final int channel) {
+ super(moduleNumber);
+ m_channel = channel;
+
+ SensorUtil.checkSolenoidModule(m_moduleNumber);
+ SensorUtil.checkSolenoidChannel(m_channel);
+
+ int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) m_channel);
+ m_solenoidHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+
+ HAL.report(tResourceType.kResourceType_Solenoid, m_channel, m_moduleNumber);
+ setName("Solenoid", m_moduleNumber, m_channel);
+ }
+
+ @Override
+ public void close() {
+ super.close();
+ SolenoidJNI.freeSolenoidPort(m_solenoidHandle);
+ m_solenoidHandle = 0;
+ }
+
+ /**
+ * Set the value of a solenoid.
+ *
+ * @param on True will turn the solenoid output on. False will turn the solenoid output off.
+ */
+ public void set(boolean on) {
+ SolenoidJNI.setSolenoid(m_solenoidHandle, on);
+ }
+
+ /**
+ * Read the current value of the solenoid.
+ *
+ * @return True if the solenoid output is on or false if the solenoid output is off.
+ */
+ public boolean get() {
+ return SolenoidJNI.getSolenoid(m_solenoidHandle);
+ }
+
+ /**
+ * Check if solenoid is blacklisted. If a solenoid is shorted, it is added to the blacklist and
+ * disabled until power cycle, or until faults are cleared.
+ *
+ * @return If solenoid is disabled due to short.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public boolean isBlackListed() {
+ int value = getPCMSolenoidBlackList() & (1 << m_channel);
+ return value != 0;
+ }
+
+ /**
+ * Set the pulse duration in the PCM. This is used in conjunction with
+ * the startPulse method to allow the PCM to control the timing of a pulse.
+ * The timing can be controlled in 0.01 second increments.
+ *
+ * @param durationSeconds The duration of the pulse, from 0.01 to 2.55 seconds.
+ *
+ * @see #startPulse()
+ */
+ public void setPulseDuration(double durationSeconds) {
+ long durationMS = (long) (durationSeconds * 1000);
+ SolenoidJNI.setOneShotDuration(m_solenoidHandle, durationMS);
+ }
+
+ /**
+ * Trigger the PCM to generate a pulse of the duration set in
+ * setPulseDuration.
+ *
+ * @see #setPulseDuration(double)
+ */
+ public void startPulse() {
+ SolenoidJNI.fireOneShot(m_solenoidHandle);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Solenoid");
+ builder.setActuator(true);
+ builder.setSafeState(() -> set(false));
+ builder.addBooleanProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
new file mode 100644
index 0000000..59c6c3d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.SolenoidJNI;
+
+/**
+ * SolenoidBase class is the common base class for the {@link Solenoid} and {@link DoubleSolenoid}
+ * classes.
+ */
+public abstract class SolenoidBase extends SendableBase {
+ protected final int m_moduleNumber; // The number of the solenoid module being used.
+
+ /**
+ * Constructor.
+ *
+ * @param moduleNumber The PCM CAN ID
+ */
+ public SolenoidBase(final int moduleNumber) {
+ m_moduleNumber = moduleNumber;
+ }
+
+ /**
+ * Read all 8 solenoids from the specified module as a single byte.
+ *
+ * @param moduleNumber the module number to read
+ * @return The current value of all 8 solenoids on the module.
+ */
+ public static int getAll(int moduleNumber) {
+ return SolenoidJNI.getAllSolenoids(moduleNumber);
+ }
+
+ /**
+ * Read all 8 solenoids from the module used by this solenoid as a single byte.
+ *
+ * @return The current value of all 8 solenoids on this module.
+ */
+ public int getAll() {
+ return SolenoidBase.getAll(m_moduleNumber);
+ }
+
+ /**
+ * Reads complete solenoid blacklist for all 8 solenoids as a single byte. If a solenoid is
+ * shorted, it is added to the blacklist and disabled until power cycle, or until faults are
+ * cleared.
+ *
+ * @param moduleNumber the module number to read
+ * @return The solenoid blacklist of all 8 solenoids on the module.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public static int getPCMSolenoidBlackList(int moduleNumber) {
+ return SolenoidJNI.getPCMSolenoidBlackList(moduleNumber);
+ }
+
+ /**
+ * Reads complete solenoid blacklist for all 8 solenoids as a single byte. If a solenoid is
+ * shorted, it is added to the blacklist and disabled until power cycle, or until faults are
+ * cleared.
+ *
+ * @return The solenoid blacklist of all 8 solenoids on the module.
+ * @see #clearAllPCMStickyFaults()
+ */
+ public int getPCMSolenoidBlackList() {
+ return SolenoidBase.getPCMSolenoidBlackList(m_moduleNumber);
+ }
+
+ /**
+ * If true, the common highside solenoid voltage rail is too low, most likely a solenoid channel
+ * is shorted.
+ *
+ * @param moduleNumber the module number to read
+ * @return true if PCM sticky fault is set
+ */
+ public static boolean getPCMSolenoidVoltageStickyFault(int moduleNumber) {
+ return SolenoidJNI.getPCMSolenoidVoltageStickyFault(moduleNumber);
+ }
+
+ /**
+ * If true, the common highside solenoid voltage rail is too low, most likely a solenoid channel
+ * is shorted.
+ *
+ * @return true if PCM sticky fault is set
+ */
+ public boolean getPCMSolenoidVoltageStickyFault() {
+ return SolenoidBase.getPCMSolenoidVoltageStickyFault(m_moduleNumber);
+ }
+
+ /**
+ * The common highside solenoid voltage rail is too low, most likely a solenoid channel is
+ * shorted.
+ *
+ * @param moduleNumber the module number to read
+ * @return true if PCM is in fault state.
+ */
+ public static boolean getPCMSolenoidVoltageFault(int moduleNumber) {
+ return SolenoidJNI.getPCMSolenoidVoltageFault(moduleNumber);
+ }
+
+ /**
+ * The common highside solenoid voltage rail is too low, most likely a solenoid channel is
+ * shorted.
+ *
+ * @return true if PCM is in fault state.
+ */
+ public boolean getPCMSolenoidVoltageFault() {
+ return SolenoidBase.getPCMSolenoidVoltageFault(m_moduleNumber);
+ }
+
+ /**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * <p>If a sticky fault is set, then it will be persistently cleared. Compressor drive maybe
+ * momentarily disable while flags are being cleared. Care should be taken to not call this too
+ * frequently, otherwise normal compressor functionality may be prevented.
+ *
+ * <p>If no sticky faults are set then this call will have no effect.
+ *
+ * @param moduleNumber the module number to read
+ */
+ public static void clearAllPCMStickyFaults(int moduleNumber) {
+ SolenoidJNI.clearAllPCMStickyFaults(moduleNumber);
+ }
+
+ /**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * <p>If a sticky fault is set, then it will be persistently cleared. Compressor drive maybe
+ * momentarily disable while flags are being cleared. Care should be taken to not call this too
+ * frequently, otherwise normal compressor functionality may be prevented.
+ *
+ * <p>If no sticky faults are set then this call will have no effect.
+ */
+ public void clearAllPCMStickyFaults() {
+ SolenoidBase.clearAllPCMStickyFaults(m_moduleNumber);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
new file mode 100644
index 0000000..aa7953a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * REV Robotics SPARK Speed Controller.
+ */
+public class Spark extends PWMSpeedController {
+ /**
+ * Common initialization code called by all constructors.
+ *
+ * <p>Note that the SPARK uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the Spark User Manual
+ * available from REV Robotics.
+ *
+ * <p>- 2.003ms = full "forward" - 1.55ms = the "high end" of the deadband range - 1.50ms =
+ * center of the deadband range (off) - 1.46ms = the "low end" of the deadband range - .999ms =
+ * full "reverse"
+ */
+ protected void initSpark() {
+ setBounds(2.003, 1.55, 1.50, 1.46, .999);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_RevSPARK, getChannel());
+ setName("Spark", getChannel());
+ }
+
+ /**
+ * Constructor.
+ *
+ * @param channel The PWM channel that the SPARK is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Spark(final int channel) {
+ super(channel);
+ initSpark();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
new file mode 100644
index 0000000..b90d19f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for speed controlling devices.
+ */
+public interface SpeedController extends PIDOutput {
+ /**
+ * Common interface for setting the speed of a speed controller.
+ *
+ * @param speed The speed to set. Value should be between -1.0 and 1.0.
+ */
+ void set(double speed);
+
+ /**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+ double get();
+
+ /**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion true is inverted.
+ */
+ void setInverted(boolean isInverted);
+
+ /**
+ * Common interface for returning if a speed controller is in the inverted state or not.
+ *
+ * @return isInverted The state of the inversion true is inverted.
+ */
+ boolean getInverted();
+
+ /**
+ * Disable the speed controller.
+ */
+ void disable();
+
+ /**
+ * Stops motor movement. Motor can be moved again by calling set without having to re-enable the
+ * motor.
+ */
+ void stopMotor();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
new file mode 100644
index 0000000..9158bb3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Allows multiple {@link SpeedController} objects to be linked together.
+ */
+public class SpeedControllerGroup extends SendableBase implements SpeedController {
+ private boolean m_isInverted;
+ private final SpeedController[] m_speedControllers;
+ private static int instances;
+
+ /**
+ * Create a new SpeedControllerGroup with the provided SpeedControllers.
+ *
+ * @param speedControllers The SpeedControllers to add
+ */
+ @SuppressWarnings("PMD.AvoidArrayLoops")
+ public SpeedControllerGroup(SpeedController speedController,
+ SpeedController... speedControllers) {
+ m_speedControllers = new SpeedController[speedControllers.length + 1];
+ m_speedControllers[0] = speedController;
+ addChild(speedController);
+ for (int i = 0; i < speedControllers.length; i++) {
+ m_speedControllers[i + 1] = speedControllers[i];
+ addChild(speedControllers[i]);
+ }
+ instances++;
+ setName("SpeedControllerGroup", instances);
+ }
+
+ @Override
+ public void set(double speed) {
+ for (SpeedController speedController : m_speedControllers) {
+ speedController.set(m_isInverted ? -speed : speed);
+ }
+ }
+
+ @Override
+ public double get() {
+ if (m_speedControllers.length > 0) {
+ return m_speedControllers[0].get() * (m_isInverted ? -1 : 1);
+ }
+ return 0.0;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ @Override
+ public void disable() {
+ for (SpeedController speedController : m_speedControllers) {
+ speedController.disable();
+ }
+ }
+
+ @Override
+ public void stopMotor() {
+ for (SpeedController speedController : m_speedControllers) {
+ speedController.stopMotor();
+ }
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Speed Controller");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Value", this::get, this::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
new file mode 100644
index 0000000..334d9e3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
+ */
+public class Talon extends PWMSpeedController {
+ /**
+ * Constructor for a Talon (original or Talon SR).
+ *
+ * <p>Note that the Talon uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the Talon User Manual
+ * available from CTRE.
+ *
+ * <p>- 2.037ms = full "forward" - 1.539ms = the "high end" of the deadband range - 1.513ms =
+ * center of the deadband range (off) - 1.487ms = the "low end" of the deadband range - .989ms
+ * = full "reverse"
+ *
+ * @param channel The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on
+ * the MXP port
+ */
+ public Talon(final int channel) {
+ super(channel);
+
+ setBounds(2.037, 1.539, 1.513, 1.487, .989);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_Talon, getChannel());
+ setName("Talon", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
new file mode 100644
index 0000000..7b49d4d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.ThreadsJNI;
+
+public final class Threads {
+ /**
+ * Get the thread priority for the current thread.
+ * @return The current thread priority. Scaled 1-99.
+ */
+ public static int getCurrentThreadPriority() {
+ return ThreadsJNI.getCurrentThreadPriority();
+ }
+
+ /**
+ * Get if the current thread is realtime.
+ * @return If the current thread is realtime
+ */
+ public static boolean getCurrentThreadIsRealTime() {
+ return ThreadsJNI.getCurrentThreadIsRealTime();
+ }
+
+ /**
+ * Sets the thread priority for the current thread.
+ *
+ * @param realTime Set to true to set a realtime priority, false for standard
+ * priority
+ * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
+ * highest. On RoboRIO, priority is ignored for non realtime setting
+ *
+ * @return The success state of setting the priority
+ */
+ public static boolean setCurrentThreadPriority(boolean realTime, int priority) {
+ return ThreadsJNI.setCurrentThreadPriority(realTime, priority);
+ }
+
+ private Threads() {
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
new file mode 100644
index 0000000..9575bf1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.NotifierJNI;
+
+/**
+ * TimedRobot implements the IterativeRobotBase robot program framework.
+ *
+ * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
+ */
+public class TimedRobot extends IterativeRobotBase {
+ public static final double kDefaultPeriod = 0.02;
+
+ // The C pointer to the notifier object. We don't use it directly, it is
+ // just passed to the JNI bindings.
+ private final int m_notifier = NotifierJNI.initializeNotifier();
+
+ // The absolute expiration time
+ private double m_expirationTime;
+
+ /**
+ * Constructor for TimedRobot.
+ */
+ protected TimedRobot() {
+ this(kDefaultPeriod);
+ }
+
+ /**
+ * Constructor for TimedRobot.
+ *
+ * @param period Period in seconds.
+ */
+ protected TimedRobot(double period) {
+ super(period);
+
+ HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed);
+ }
+
+ @Override
+ @SuppressWarnings("NoFinalizer")
+ protected void finalize() {
+ NotifierJNI.stopNotifier(m_notifier);
+ NotifierJNI.cleanNotifier(m_notifier);
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ @Override
+ @SuppressWarnings("UnsafeFinalization")
+ public void startCompetition() {
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period;
+ updateAlarm();
+
+ // Loop forever, calling the appropriate mode-dependent function
+ while (true) {
+ long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
+ if (curTime == 0) {
+ break;
+ }
+
+ m_expirationTime += m_period;
+ updateAlarm();
+
+ loopFunc();
+ }
+ }
+
+ /**
+ * Get time period between calls to Periodic() functions.
+ */
+ public double getPeriod() {
+ return m_period;
+ }
+
+ /**
+ * Update the alarm hardware to reflect the next alarm.
+ */
+ @SuppressWarnings("UnsafeFinalization")
+ private void updateAlarm() {
+ NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
new file mode 100644
index 0000000..a4c553e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+public class Timer {
+ /**
+ * Return the system clock time in seconds. Return the time from the FPGA hardware clock in
+ * seconds since the FPGA started.
+ *
+ * @return Robot running time in seconds.
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static double getFPGATimestamp() {
+ return RobotController.getFPGATime() / 1000000.0;
+ }
+
+ /**
+ * Return the approximate match time. The FMS does not send an official match time to the robots,
+ * but does send an approximate match time. The value will count down the time remaining in the
+ * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
+ * dispute ref calls or guarantee that a function will trigger before the match ends) The
+ * Practice Match function of the DS approximates the behaviour seen on the field.
+ *
+ * @return Time remaining in current match period (auto or teleop) in seconds
+ */
+ public static double getMatchTime() {
+ return DriverStation.getInstance().getMatchTime();
+ }
+
+ /**
+ * Pause the thread for a specified time. Pause the execution of the thread for a specified period
+ * of time given in seconds. Motors will continue to run at their last assigned values, and
+ * sensors will continue to update. Only the task containing the wait will pause until the wait
+ * time is expired.
+ *
+ * @param seconds Length of time to pause
+ */
+ public static void delay(final double seconds) {
+ try {
+ Thread.sleep((long) (seconds * 1e3));
+ } catch (final InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ }
+
+ private double m_startTime;
+ private double m_accumulatedTime;
+ private boolean m_running;
+
+ @SuppressWarnings("JavadocMethod")
+ public Timer() {
+ reset();
+ }
+
+ private double getMsClock() {
+ return RobotController.getFPGATime() / 1000.0;
+ }
+
+ /**
+ * Get the current time from the timer. If the clock is running it is derived from the current
+ * system clock the start time stored in the timer class. If the clock is not running, then return
+ * the time when it was last stopped.
+ *
+ * @return Current time value for this timer in seconds
+ */
+ public synchronized double get() {
+ if (m_running) {
+ return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
+ } else {
+ return m_accumulatedTime;
+ }
+ }
+
+ /**
+ * Reset the timer by setting the time to 0. Make the timer startTime the current time so new
+ * requests will be relative now
+ */
+ public synchronized void reset() {
+ m_accumulatedTime = 0;
+ m_startTime = getMsClock();
+ }
+
+ /**
+ * Start the timer running. Just set the running flag to true indicating that all time requests
+ * should be relative to the system clock.
+ */
+ public synchronized void start() {
+ m_startTime = getMsClock();
+ m_running = true;
+ }
+
+ /**
+ * Stop the timer. This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than looking at the system
+ * clock.
+ */
+ public synchronized void stop() {
+ final double temp = get();
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+
+ /**
+ * Check if the period specified has passed and if it has, advance the start time by that period.
+ * This is useful to decide if it's time to do periodic work without drifting later by the time it
+ * took to get around to checking.
+ *
+ * @param period The period to check for (in seconds).
+ * @return If the period has passed.
+ */
+ public synchronized boolean hasPeriodPassed(double period) {
+ if (get() > period) {
+ // Advance the start time by the period.
+ // Don't set it to the current time... we want to avoid drift.
+ m_startTime += period * 1000;
+ return true;
+ }
+ return false;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
new file mode 100644
index 0000000..b42dfdf
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -0,0 +1,398 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Ultrasonic rangefinder class. The Ultrasonic rangefinder measures absolute distance based on the
+ * round-trip time of a ping generated by the controller. These sensors use two transducers, a
+ * speaker and a microphone both tuned to the ultrasonic range. A common ultrasonic sensor, the
+ * Daventech SRF04 requires a short pulse to be generated on a digital channel. This causes the
+ * chirp to be emitted. A second line becomes high as the ping is transmitted and goes low when the
+ * echo is received. The time that the line is high determines the round trip distance (time of
+ * flight).
+ */
+public class Ultrasonic extends SendableBase implements PIDSource {
+ /**
+ * The units to return when PIDGet is called.
+ */
+ public enum Unit {
+ /**
+ * Use inches for PIDGet.
+ */
+ kInches,
+ /**
+ * Use millimeters for PIDGet.
+ */
+ kMillimeters
+ }
+
+ // Time (sec) for the ping trigger pulse.
+ private static final double kPingTime = 10 * 1e-6;
+ private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
+ // ultrasonic sensor list
+ private static final List<Ultrasonic> m_sensors = new ArrayList<>();
+ // automatic round robin mode
+ private static boolean m_automaticEnabled;
+ private DigitalInput m_echoChannel;
+ private DigitalOutput m_pingChannel;
+ private boolean m_allocatedChannels;
+ private boolean m_enabled;
+ private Counter m_counter;
+ // task doing the round-robin automatic sensing
+ private static Thread m_task;
+ private Unit m_units;
+ private static int m_instances;
+ protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+ /**
+ * Background task that goes through the list of ultrasonic sensors and pings each one in turn.
+ * The counter is configured to read the timing of the returned echo pulse.
+ *
+ * <p><b>DANGER WILL ROBINSON, DANGER WILL ROBINSON:</b> This code runs as a task and assumes that
+ * none of the ultrasonic sensors will change while it's running. If one does, then this will
+ * certainly break. Make sure to disable automatic mode before changing anything with the
+ * sensors!!
+ */
+ private static class UltrasonicChecker extends Thread {
+ @Override
+ public synchronized void run() {
+ int sensorIndex = 0;
+ Ultrasonic ultrasonic;
+ while (m_automaticEnabled) {
+ //lock list to ensure deletion doesn't occur between empty check and retrieving sensor
+ synchronized (m_sensors) {
+ if (m_sensors.isEmpty()) {
+ return;
+ }
+ if (sensorIndex >= m_sensors.size()) {
+ sensorIndex = m_sensors.size() - 1;
+ }
+ ultrasonic = m_sensors.get(sensorIndex);
+ }
+ if (ultrasonic.isEnabled()) {
+ // Do the ping
+ ultrasonic.m_pingChannel.pulse(kPingTime);
+ }
+ if (sensorIndex < m_sensors.size()) {
+ sensorIndex++;
+ } else {
+ sensorIndex = 0;
+ }
+
+ Timer.delay(.1); // wait for ping to return
+ }
+ }
+ }
+
+ /**
+ * Initialize the Ultrasonic Sensor. This is the common code that initializes the ultrasonic
+ * sensor given that there are two digital I/O channels allocated. If the system was running in
+ * automatic mode (round robin) when the new sensor is added, it is stopped, the sensor is added,
+ * then automatic mode is restored.
+ */
+ private synchronized void initialize() {
+ if (m_task == null) {
+ m_task = new UltrasonicChecker();
+ }
+ final boolean originalMode = m_automaticEnabled;
+ setAutomaticMode(false); // kill task when adding a new sensor
+ m_sensors.add(this);
+
+ m_counter = new Counter(m_echoChannel); // set up counter for this
+ addChild(m_counter);
+ // sensor
+ m_counter.setMaxPeriod(1.0);
+ m_counter.setSemiPeriodMode(true);
+ m_counter.reset();
+ m_enabled = true; // make it available for round robin scheduling
+ setAutomaticMode(originalMode);
+
+ m_instances++;
+ HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances);
+ setName("Ultrasonic", m_echoChannel.getChannel());
+ }
+
+ /**
+ * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
+ * and Vex ultrasonic sensors.
+ *
+ * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
+ * sending the ping.
+ * @param echoChannel The digital input channel that receives the echo. The length of time that
+ * the echo is high represents the round trip time of the ping, and the
+ * distance.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+ public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
+ m_pingChannel = new DigitalOutput(pingChannel);
+ m_echoChannel = new DigitalInput(echoChannel);
+ addChild(m_pingChannel);
+ addChild(m_echoChannel);
+ m_allocatedChannels = true;
+ m_units = units;
+ initialize();
+ }
+
+ /**
+ * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
+ * and Vex ultrasonic sensors. Default unit is inches.
+ *
+ * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
+ * sending the ping.
+ * @param echoChannel The digital input channel that receives the echo. The length of time that
+ * the echo is high represents the round trip time of the ping, and the
+ * distance.
+ */
+ public Ultrasonic(final int pingChannel, final int echoChannel) {
+ this(pingChannel, echoChannel, Unit.kInches);
+ }
+
+ /**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
+ * DigitalOutput for the ping channel.
+ *
+ * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
+ * 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to determine the
+ * range.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+ public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
+ requireNonNull(pingChannel, "Provided ping channel was null");
+ requireNonNull(echoChannel, "Provided echo channel was null");
+
+ m_allocatedChannels = false;
+ m_pingChannel = pingChannel;
+ m_echoChannel = echoChannel;
+ m_units = units;
+ initialize();
+ }
+
+ /**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
+ * DigitalOutput for the ping channel. Default unit is inches.
+ *
+ * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
+ * 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to determine the
+ * range.
+ */
+ public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
+ this(pingChannel, echoChannel, Unit.kInches);
+ }
+
+ /**
+ * Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing
+ * the allocated digital channels. If the system was in automatic mode (round robin), then it is
+ * stopped, then started again after this sensor is removed (provided this wasn't the last
+ * sensor).
+ */
+ @Override
+ public synchronized void close() {
+ super.close();
+ final boolean wasAutomaticMode = m_automaticEnabled;
+ setAutomaticMode(false);
+ if (m_allocatedChannels) {
+ if (m_pingChannel != null) {
+ m_pingChannel.close();
+ }
+ if (m_echoChannel != null) {
+ m_echoChannel.close();
+ }
+ }
+
+ if (m_counter != null) {
+ m_counter.close();
+ m_counter = null;
+ }
+
+ m_pingChannel = null;
+ m_echoChannel = null;
+ synchronized (m_sensors) {
+ m_sensors.remove(this);
+ }
+ if (!m_sensors.isEmpty() && wasAutomaticMode) {
+ setAutomaticMode(true);
+ }
+ }
+
+ /**
+ * Turn Automatic mode on/off. When in Automatic mode, all sensors will fire in round robin,
+ * waiting a set time between each sensor.
+ *
+ * @param enabling Set to true if round robin scheduling should start for all the ultrasonic
+ * sensors. This scheduling method assures that the sensors are non-interfering
+ * because no two sensors fire at the same time. If another scheduling algorithm
+ * is preferred, it can be implemented by pinging the sensors manually and waiting
+ * for the results to come back.
+ */
+ public void setAutomaticMode(boolean enabling) {
+ if (enabling == m_automaticEnabled) {
+ return; // ignore the case of no change
+ }
+ m_automaticEnabled = enabling;
+
+ if (enabling) {
+ /* Clear all the counters so no data is valid. No synchronization is
+ * needed because the background task is stopped.
+ */
+ for (Ultrasonic u : m_sensors) {
+ u.m_counter.reset();
+ }
+
+ // Start round robin task
+ m_task.start();
+ } else {
+ // Wait for background task to stop running
+ try {
+ m_task.join();
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ ex.printStackTrace();
+ }
+
+ /* Clear all the counters (data now invalid) since automatic mode is
+ * disabled. No synchronization is needed because the background task is
+ * stopped.
+ */
+ for (Ultrasonic u : m_sensors) {
+ u.m_counter.reset();
+ }
+ }
+ }
+
+ /**
+ * Single ping to ultrasonic sensor. Send out a single ping to the ultrasonic sensor. This only
+ * works if automatic (round robin) mode is disabled. A single ping is sent out, and the counter
+ * should count the semi-period when it comes in. The counter is reset to make the current value
+ * invalid.
+ */
+ public void ping() {
+ setAutomaticMode(false); // turn off automatic round robin if pinging
+ // single sensor
+ m_counter.reset(); // reset the counter to zero (invalid data now)
+ // do the ping to start getting a single range
+ m_pingChannel.pulse(kPingTime);
+ }
+
+ /**
+ * Check if there is a valid range measurement. The ranges are accumulated in a counter that will
+ * increment on each edge of the echo (return) signal. If the count is not at least 2, then the
+ * range has not yet been measured, and is invalid.
+ *
+ * @return true if the range is valid
+ */
+ public boolean isRangeValid() {
+ return m_counter.get() > 1;
+ }
+
+ /**
+ * Get the range in inches from the ultrasonic sensor. If there is no valid value yet, i.e. at
+ * least one measurement hasn't completed, then return 0.
+ *
+ * @return double Range in inches of the target returned from the ultrasonic sensor.
+ */
+ public double getRangeInches() {
+ if (isRangeValid()) {
+ return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
+ } else {
+ return 0;
+ }
+ }
+
+ /**
+ * Get the range in millimeters from the ultrasonic sensor. If there is no valid value yet, i.e.
+ * at least one measurement hasn't completed, then return 0.
+ *
+ * @return double Range in millimeters of the target returned by the ultrasonic sensor.
+ */
+ public double getRangeMM() {
+ return getRangeInches() * 25.4;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+ throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
+ }
+ m_pidSource = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_pidSource;
+ }
+
+ /**
+ * Get the range in the current DistanceUnit for the PIDSource base object.
+ *
+ * @return The range in DistanceUnit
+ */
+ @Override
+ public double pidGet() {
+ switch (m_units) {
+ case kInches:
+ return getRangeInches();
+ case kMillimeters:
+ return getRangeMM();
+ default:
+ return 0.0;
+ }
+ }
+
+ /**
+ * Set the current DistanceUnit that should be used for the PIDSource base object.
+ *
+ * @param units The DistanceUnit that should be used.
+ */
+ public void setDistanceUnits(Unit units) {
+ m_units = units;
+ }
+
+ /**
+ * Get the current DistanceUnit that is used for the PIDSource base object.
+ *
+ * @return The type of DistanceUnit that is being used.
+ */
+ public Unit getDistanceUnits() {
+ return m_units;
+ }
+
+ /**
+ * Is the ultrasonic enabled.
+ *
+ * @return true if the ultrasonic is enabled
+ */
+ public boolean isEnabled() {
+ return m_enabled;
+ }
+
+ /**
+ * Set if the ultrasonic is enabled.
+ *
+ * @param enable set to true to enable the ultrasonic
+ */
+ public void setEnabled(boolean enable) {
+ m_enabled = enable;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Ultrasonic");
+ builder.addDoubleProperty("Value", this::getRangeInches, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Utility.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Utility.java
new file mode 100644
index 0000000..47035b4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Utility.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HALUtil;
+
+/**
+ * Contains global utility functions.
+ * @deprecated Use RobotController class instead
+ */
+@Deprecated
+public final class Utility {
+ private Utility() {
+ }
+
+ /**
+ * Return the FPGA Version number. For now, expect this to be 2009.
+ *
+ * @return FPGA Version number.
+ * @deprecated Use RobotController.getFPGAVersion()
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ @Deprecated
+ int getFPGAVersion() {
+ return HALUtil.getFPGAVersion();
+ }
+
+ /**
+ * Return the FPGA Revision number. The format of the revision is 3 numbers. The 12 most
+ * significant bits are the Major Revision. the next 8 bits are the Minor Revision. The 12 least
+ * significant bits are the Build Number.
+ *
+ * @return FPGA Revision number.
+ * @deprecated Use RobotController.getFPGARevision()
+ */
+ @SuppressWarnings("AbbreviationAsWordInName")
+ @Deprecated
+ long getFPGARevision() {
+ return (long) HALUtil.getFPGARevision();
+ }
+
+ /**
+ * Read the microsecond timer from the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA.
+ * @deprecated Use RobotController.getFPGATime()
+ */
+ @Deprecated
+ @SuppressWarnings("AbbreviationAsWordInName")
+ public static long getFPGATime() {
+ return HALUtil.getFPGATime();
+ }
+
+ /**
+ * Get the state of the "USER" button on the roboRIO.
+ *
+ * @return true if the button is currently pressed down
+ * @deprecated Use RobotController.getUserButton()
+ */
+ @Deprecated
+ public static boolean getUserButton() {
+ return HALUtil.getFPGAButton();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
new file mode 100644
index 0000000..b1aeeab
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * VEX Robotics Victor 888 Speed Controller The Vex Robotics Victor 884 Speed Controller can also
+ * be used with this class but may need to be calibrated per the Victor 884 user manual.
+ */
+public class Victor extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * <p>Note that the Victor uses the following bounds for PWM values. These values were determined
+ * empirically and optimized for the Victor 888. These values should work reasonably well for
+ * Victor 884 controllers also but if users experience issues such as asymmetric behaviour around
+ * the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User Manual available
+ * from VEX Robotics: http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf
+ *
+ * <p>- 2.027ms = full "forward" - 1.525ms = the "high end" of the deadband range - 1.507ms =
+ * center of the deadband range (off) - 1.49ms = the "low end" of the deadband range - 1.026ms =
+ * full "reverse"
+ *
+ * @param channel The PWM channel that the Victor is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ public Victor(final int channel) {
+ super(channel);
+
+ setBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+ setPeriodMultiplier(PeriodMultiplier.k2X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_Victor, getChannel());
+ setName("Victor", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
new file mode 100644
index 0000000..3de2a0a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * VEX Robotics Victor SP Speed Controller.
+ */
+public class VictorSP extends PWMSpeedController {
+ /**
+ * Constructor.
+ *
+ * <p>Note that the VictorSP uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric
+ * behavior around the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended. The calibration procedure can be found in the VictorSP User Manual
+ * available from CTRE.
+ *
+ * <p>- 2.004ms = full "forward" - 1.52ms = the "high end" of the deadband range - 1.50ms =
+ * center of the deadband range (off) - 1.48ms = the "low end" of the deadband range - .997ms =
+ * full "reverse"
+ *
+ * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ public VictorSP(final int channel) {
+ super(channel);
+
+ setBounds(2.004, 1.52, 1.50, 1.48, .997);
+ setPeriodMultiplier(PeriodMultiplier.k1X);
+ setSpeed(0.0);
+ setZeroLatch();
+
+ HAL.report(tResourceType.kResourceType_VictorSP, getChannel());
+ setName("VictorSP", getChannel());
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
new file mode 100644
index 0000000..7ff8937
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
@@ -0,0 +1,292 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.Closeable;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.PriorityQueue;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
+
+/**
+ * A class that's a wrapper around a watchdog timer.
+ *
+ * <p>When the timer expires, a message is printed to the console and an optional user-provided
+ * callback is invoked.
+ *
+ * <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class Watchdog implements Closeable, Comparable<Watchdog> {
+ // Used for timeout print rate-limiting
+ private static final long kMinPrintPeriod = 1000000; // us
+
+ private long m_startTime; // us
+ private long m_timeout; // us
+ private long m_expirationTime; // us
+ private final Runnable m_callback;
+ private long m_lastTimeoutPrintTime; // us
+ private long m_lastEpochsPrintTime; // us
+
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private final Map<String, Long> m_epochs = new HashMap<>();
+ boolean m_isExpired;
+
+ boolean m_suppressTimeoutMessage;
+
+ static {
+ startDaemonThread(() -> schedulerFunc());
+ }
+
+ private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
+ private static ReentrantLock m_queueMutex = new ReentrantLock();
+ private static Condition m_schedulerWaiter = m_queueMutex.newCondition();
+
+ /**
+ * Watchdog constructor.
+ *
+ * @param timeout The watchdog's timeout in seconds with microsecond resolution.
+ * @param callback This function is called when the timeout expires.
+ */
+ public Watchdog(double timeout, Runnable callback) {
+ m_timeout = (long) (timeout * 1.0e6);
+ m_callback = callback;
+ }
+
+ @Override
+ public void close() {
+ disable();
+ }
+
+ @Override
+ public int compareTo(Watchdog rhs) {
+ // Elements with sooner expiration times are sorted as lesser. The head of
+ // Java's PriorityQueue is the least element.
+ if (m_expirationTime < rhs.m_expirationTime) {
+ return -1;
+ } else if (m_expirationTime > rhs.m_expirationTime) {
+ return 1;
+ } else {
+ return 0;
+ }
+ }
+
+ /**
+ * Returns the time in seconds since the watchdog was last fed.
+ */
+ public double getTime() {
+ return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
+ }
+
+ /**
+ * Sets the watchdog's timeout.
+ *
+ * @param timeout The watchdog's timeout in seconds with microsecond
+ * resolution.
+ */
+ public void setTimeout(double timeout) {
+ m_startTime = RobotController.getFPGATime();
+ m_epochs.clear();
+
+ m_queueMutex.lock();
+ try {
+ m_timeout = (long) (timeout * 1.0e6);
+ m_isExpired = false;
+
+ m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + m_timeout;
+ m_watchdogs.add(this);
+ m_schedulerWaiter.signalAll();
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns the watchdog's timeout in seconds.
+ */
+ public double getTimeout() {
+ m_queueMutex.lock();
+ try {
+ return m_timeout / 1.0e6;
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Returns true if the watchdog timer has expired.
+ */
+ public boolean isExpired() {
+ m_queueMutex.lock();
+ try {
+ return m_isExpired;
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Adds time since last epoch to the list printed by printEpochs().
+ *
+ * <p>Epochs are a way to partition the time elapsed so that when overruns occur, one can
+ * determine which parts of an operation consumed the most time.
+ *
+ * @param epochName The name to associate with the epoch.
+ */
+ public void addEpoch(String epochName) {
+ long currentTime = RobotController.getFPGATime();
+ m_epochs.put(epochName, currentTime - m_startTime);
+ m_startTime = currentTime;
+ }
+
+ /**
+ * Prints list of epochs added so far and their times.
+ */
+ public void printEpochs() {
+ long now = RobotController.getFPGATime();
+ if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+ m_lastEpochsPrintTime = now;
+ m_epochs.forEach((key, value) -> {
+ System.out.format("\t%s: %.6fs\n", key, value / 1.0e6);
+ });
+ }
+ }
+
+ /**
+ * Resets the watchdog timer.
+ *
+ * <p>This also enables the timer if it was previously disabled.
+ */
+ public void reset() {
+ enable();
+ }
+
+ /**
+ * Enables the watchdog timer.
+ */
+ public void enable() {
+ m_startTime = RobotController.getFPGATime();
+ m_epochs.clear();
+
+ m_queueMutex.lock();
+ try {
+ m_isExpired = false;
+
+ m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + m_timeout;
+ m_watchdogs.add(this);
+ m_schedulerWaiter.signalAll();
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Disables the watchdog timer.
+ */
+ public void disable() {
+ m_queueMutex.lock();
+ try {
+ m_isExpired = false;
+
+ m_watchdogs.remove(this);
+ m_schedulerWaiter.signalAll();
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Enable or disable suppression of the generic timeout message.
+ *
+ * <p>This may be desirable if the user-provided callback already prints a more specific message.
+ *
+ * @param suppress Whether to suppress generic timeout message.
+ */
+ public void suppressTimeoutMessage(boolean suppress) {
+ m_suppressTimeoutMessage = suppress;
+ }
+
+ private static Thread startDaemonThread(Runnable target) {
+ Thread inst = new Thread(target);
+ inst.setDaemon(true);
+ inst.start();
+ return inst;
+ }
+
+
+ private static void schedulerFunc() {
+ m_queueMutex.lock();
+
+ try {
+ while (true) {
+ if (m_watchdogs.size() > 0) {
+ boolean timedOut = !awaitUntil(m_schedulerWaiter, m_watchdogs.peek().m_expirationTime);
+ if (timedOut) {
+ if (m_watchdogs.size() == 0 || m_watchdogs.peek().m_expirationTime
+ > RobotController.getFPGATime()) {
+ continue;
+ }
+
+ // If the condition variable timed out, that means a Watchdog timeout
+ // has occurred, so call its timeout function.
+ Watchdog watchdog = m_watchdogs.poll();
+
+ long now = RobotController.getFPGATime();
+ if (now - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
+ watchdog.m_lastTimeoutPrintTime = now;
+ if (!watchdog.m_suppressTimeoutMessage) {
+ System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
+ }
+ }
+
+ // Set expiration flag before calling the callback so any
+ // manipulation of the flag in the callback (e.g., calling
+ // Disable()) isn't clobbered.
+ watchdog.m_isExpired = true;
+
+ m_queueMutex.unlock();
+ watchdog.m_callback.run();
+ m_queueMutex.lock();
+ }
+ // Otherwise, a Watchdog removed itself from the queue (it notifies
+ // the scheduler of this) or a spurious wakeup occurred, so just
+ // rewait with the soonest watchdog timeout.
+ } else {
+ while (m_watchdogs.size() == 0) {
+ m_schedulerWaiter.awaitUninterruptibly();
+ }
+ }
+ }
+ } finally {
+ m_queueMutex.unlock();
+ }
+ }
+
+ /**
+ * Wrapper emulating functionality of C++'s std::condition_variable::wait_until().
+ *
+ * @param cond The condition variable on which to wait.
+ * @param time The time at which to stop waiting.
+ * @return False if the deadline has elapsed upon return, else true.
+ */
+ private static boolean awaitUntil(Condition cond, long time) {
+ long delta = time - RobotController.getFPGATime();
+ try {
+ return cond.await(delta, TimeUnit.MICROSECONDS);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ ex.printStackTrace();
+ }
+
+ return true;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
new file mode 100644
index 0000000..79d80e6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -0,0 +1,345 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Handle input from Xbox 360 or Xbox One controllers connected to the Driver Station.
+ *
+ * <p>This class handles Xbox input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each controller
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class XboxController extends GenericHID {
+ /**
+ * Represents a digital button on an XboxController.
+ */
+ private enum Button {
+ kBumperLeft(5),
+ kBumperRight(6),
+ kStickLeft(9),
+ kStickRight(10),
+ kA(1),
+ kB(2),
+ kX(3),
+ kY(4),
+ kBack(7),
+ kStart(8);
+
+ @SuppressWarnings({"MemberName", "PMD.SingularField"})
+ private final int value;
+
+ Button(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * Construct an instance of a joystick. The joystick index is the USB port on the drivers
+ * station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into.
+ */
+ public XboxController(final int port) {
+ super(port);
+
+ HAL.report(tResourceType.kResourceType_XboxController, port);
+ }
+
+ /**
+ * Get the X axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The X axis value of the controller.
+ */
+ @Override
+ public double getX(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawAxis(0);
+ } else {
+ return getRawAxis(4);
+ }
+ }
+
+ /**
+ * Get the Y axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The Y axis value of the controller.
+ */
+ @Override
+ public double getY(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawAxis(1);
+ } else {
+ return getRawAxis(5);
+ }
+ }
+
+ /**
+ * Get the trigger axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The trigger axis value of the controller.
+ */
+ public double getTriggerAxis(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawAxis(2);
+ } else {
+ return getRawAxis(3);
+ }
+ }
+
+ /**
+ * Read the value of the bumper button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+ public boolean getBumper(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawButton(Button.kBumperLeft.value);
+ } else {
+ return getRawButton(Button.kBumperRight.value);
+ }
+ }
+
+ /**
+ * Whether the bumper was pressed since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getBumperPressed(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonPressed(Button.kBumperLeft.value);
+ } else {
+ return getRawButtonPressed(Button.kBumperRight.value);
+ }
+ }
+
+ /**
+ * Whether the bumper was released since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getBumperReleased(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonReleased(Button.kBumperLeft.value);
+ } else {
+ return getRawButtonReleased(Button.kBumperRight.value);
+ }
+ }
+
+ /**
+ * Read the value of the stick button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+ public boolean getStickButton(Hand hand) {
+ if (hand.equals(Hand.kLeft)) {
+ return getRawButton(Button.kStickLeft.value);
+ } else {
+ return getRawButton(Button.kStickRight.value);
+ }
+ }
+
+ /**
+ * Whether the stick button was pressed since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getStickButtonPressed(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonPressed(Button.kStickLeft.value);
+ } else {
+ return getRawButtonPressed(Button.kStickRight.value);
+ }
+ }
+
+ /**
+ * Whether the stick button was released since the last check.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getStickButtonReleased(Hand hand) {
+ if (hand == Hand.kLeft) {
+ return getRawButtonReleased(Button.kStickLeft.value);
+ } else {
+ return getRawButtonReleased(Button.kStickRight.value);
+ }
+ }
+
+ /**
+ * Read the value of the A button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getAButton() {
+ return getRawButton(Button.kA.value);
+ }
+
+ /**
+ * Whether the A button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getAButtonPressed() {
+ return getRawButtonPressed(Button.kA.value);
+ }
+
+ /**
+ * Whether the A button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getAButtonReleased() {
+ return getRawButtonReleased(Button.kA.value);
+ }
+
+ /**
+ * Read the value of the B button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getBButton() {
+ return getRawButton(Button.kB.value);
+ }
+
+ /**
+ * Whether the B button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getBButtonPressed() {
+ return getRawButtonPressed(Button.kB.value);
+ }
+
+ /**
+ * Whether the B button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getBButtonReleased() {
+ return getRawButtonReleased(Button.kB.value);
+ }
+
+ /**
+ * Read the value of the X button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getXButton() {
+ return getRawButton(Button.kX.value);
+ }
+
+ /**
+ * Whether the X button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getXButtonPressed() {
+ return getRawButtonPressed(Button.kX.value);
+ }
+
+ /**
+ * Whether the X button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getXButtonReleased() {
+ return getRawButtonReleased(Button.kX.value);
+ }
+
+ /**
+ * Read the value of the Y button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getYButton() {
+ return getRawButton(Button.kY.value);
+ }
+
+ /**
+ * Whether the Y button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getYButtonPressed() {
+ return getRawButtonPressed(Button.kY.value);
+ }
+
+ /**
+ * Whether the Y button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getYButtonReleased() {
+ return getRawButtonReleased(Button.kY.value);
+ }
+
+ /**
+ * Read the value of the back button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getBackButton() {
+ return getRawButton(Button.kBack.value);
+ }
+
+ /**
+ * Whether the back button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getBackButtonPressed() {
+ return getRawButtonPressed(Button.kBack.value);
+ }
+
+ /**
+ * Whether the back button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getBackButtonReleased() {
+ return getRawButtonReleased(Button.kBack.value);
+ }
+
+ /**
+ * Read the value of the start button on the controller.
+ *
+ * @return The state of the button.
+ */
+ public boolean getStartButton() {
+ return getRawButton(Button.kStart.value);
+ }
+
+ /**
+ * Whether the start button was pressed since the last check.
+ *
+ * @return Whether the button was pressed since the last check.
+ */
+ public boolean getStartButtonPressed() {
+ return getRawButtonPressed(Button.kStart.value);
+ }
+
+ /**
+ * Whether the start button was released since the last check.
+ *
+ * @return Whether the button was released since the last check.
+ */
+ public boolean getStartButtonReleased() {
+ return getRawButtonReleased(Button.kStart.value);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
new file mode 100644
index 0000000..a7dc2fa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger button
+ * of a joystick to a "score" command.
+ *
+ * <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
+ * operator interface as a common use case of the more generalized Trigger objects. This is a simple
+ * wrapper around Trigger with the method names renamed to fit the Button object use.
+ */
+public abstract class Button extends Trigger {
+ /**
+ * Starts the given command whenever the button is newly pressed.
+ *
+ * @param command the command to start
+ */
+ public void whenPressed(final Command command) {
+ whenActive(command);
+ }
+
+ /**
+ * Constantly starts the given command while the button is held.
+ *
+ * {@link Command#start()} will be called repeatedly while the button is held, and will be
+ * canceled when the button is released.
+ *
+ * @param command the command to start
+ */
+ public void whileHeld(final Command command) {
+ whileActive(command);
+ }
+
+ /**
+ * Starts the command when the button is released.
+ *
+ * @param command the command to start
+ */
+ public void whenReleased(final Command command) {
+ whenInactive(command);
+ }
+
+ /**
+ * Toggles the command whenever the button is pressed (on then off then on).
+ *
+ * @param command the command to start
+ */
+ public void toggleWhenPressed(final Command command) {
+ toggleWhenActive(command);
+ }
+
+ /**
+ * Cancel the command when the button is pressed.
+ *
+ * @param command the command to start
+ */
+ public void cancelWhenPressed(final Command command) {
+ cancelWhenActive(command);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
new file mode 100644
index 0000000..bb1eff7
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+/**
+ * This class is intended to be used within a program. The programmer can manually set its value.
+ * Also includes a setting for whether or not it should invert its value.
+ */
+public class InternalButton extends Button {
+ private boolean m_pressed;
+ private boolean m_inverted;
+
+ /**
+ * Creates an InternalButton that is not inverted.
+ */
+ public InternalButton() {
+ this(false);
+ }
+
+ /**
+ * Creates an InternalButton which is inverted depending on the input.
+ *
+ * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
+ * when set to false.
+ */
+ public InternalButton(boolean inverted) {
+ m_pressed = m_inverted = inverted;
+ }
+
+ public void setInverted(boolean inverted) {
+ m_inverted = inverted;
+ }
+
+ public void setPressed(boolean pressed) {
+ m_pressed = pressed;
+ }
+
+ @Override
+ public boolean get() {
+ return m_pressed ^ m_inverted;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
new file mode 100644
index 0000000..db0d4c6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * A {@link Button} that gets its state from a {@link GenericHID}.
+ */
+public class JoystickButton extends Button {
+ private final GenericHID m_joystick;
+ private final int m_buttonNumber;
+
+ /**
+ * Create a joystick button for triggering commands.
+ *
+ * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick,
+ * etc)
+ * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
+ */
+ public JoystickButton(GenericHID joystick, int buttonNumber) {
+ m_joystick = joystick;
+ m_buttonNumber = buttonNumber;
+ }
+
+ /**
+ * Gets the value of the joystick button.
+ *
+ * @return The value of the joystick button
+ */
+ @Override
+ public boolean get() {
+ return m_joystick.getRawButton(m_buttonNumber);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
new file mode 100644
index 0000000..2fa7924
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * A {@link Button} that uses a {@link NetworkTable} boolean field.
+ */
+public class NetworkButton extends Button {
+ private final NetworkTableEntry m_entry;
+
+ public NetworkButton(String table, String field) {
+ this(NetworkTableInstance.getDefault().getTable(table), field);
+ }
+
+ public NetworkButton(NetworkTable table, String field) {
+ m_entry = table.getEntry(field);
+ }
+
+ @Override
+ public boolean get() {
+ return m_entry.getInstance().isConnected() && m_entry.getBoolean(false);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
new file mode 100644
index 0000000..abf8ca7
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * A {@link Button} that gets its state from a POV on a {@link GenericHID}.
+ */
+public class POVButton extends Button {
+ private final GenericHID m_joystick;
+ private final int m_angle;
+ private final int m_povNumber;
+
+ /**
+ * Creates a POV button for triggering commands.
+ *
+ * @param joystick The GenericHID object that has the POV
+ * @param angle The desired angle in degrees (e.g. 90, 270)
+ * @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
+ */
+ public POVButton(GenericHID joystick, int angle, int povNumber) {
+ m_joystick = joystick;
+ m_angle = angle;
+ m_povNumber = povNumber;
+ }
+
+ /**
+ * Creates a POV button for triggering commands.
+ * By default, acts on POV 0
+ *
+ * @param joystick The GenericHID object that has the POV
+ * @param angle The desired angle (e.g. 90, 270)
+ */
+ public POVButton(GenericHID joystick, int angle) {
+ this(joystick, angle, 0);
+ }
+
+ /**
+ * Checks whether the current value of the POV is the target angle.
+ *
+ * @return Whether the value of the POV matches the target angle
+ */
+ @Override
+ public boolean get() {
+ return m_joystick.getPOV(m_povNumber) == m_angle;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
new file mode 100644
index 0000000..c20e62e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
@@ -0,0 +1,185 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger
+ * button of a joystick to a "score" command.
+ *
+ * <p>It is encouraged that teams write a subclass of Trigger if they want to have something unusual
+ * (for instance, if they want to react to the user holding a button while the robot is reading a
+ * certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get
+ * the full functionality of the Trigger class.
+ */
+public abstract class Trigger extends SendableBase {
+ private volatile boolean m_sendablePressed;
+
+ /**
+ * Returns whether or not the trigger is active.
+ *
+ * <p>This method will be called repeatedly a command is linked to the Trigger.
+ *
+ * @return whether or not the trigger condition is active.
+ */
+ public abstract boolean get();
+
+ /**
+ * Returns whether get() return true or the internal table for SmartDashboard use is pressed.
+ *
+ * @return whether get() return true or the internal table for SmartDashboard use is pressed.
+ */
+ @SuppressWarnings("PMD.UselessParentheses")
+ private boolean grab() {
+ return get() || m_sendablePressed;
+ }
+
+ /**
+ * Starts the given command whenever the trigger just becomes active.
+ *
+ * @param command the command to start
+ */
+ public void whenActive(final Command command) {
+ new ButtonScheduler() {
+ private boolean m_pressedLast = grab();
+
+ @Override
+ public void execute() {
+ boolean pressed = grab();
+
+ if (!m_pressedLast && pressed) {
+ command.start();
+ }
+
+ m_pressedLast = pressed;
+ }
+ }.start();
+ }
+
+ /**
+ * Constantly starts the given command while the button is held.
+ *
+ * {@link Command#start()} will be called repeatedly while the trigger is active, and will be
+ * canceled when the trigger becomes inactive.
+ *
+ * @param command the command to start
+ */
+ public void whileActive(final Command command) {
+ new ButtonScheduler() {
+ private boolean m_pressedLast = grab();
+
+ @Override
+ public void execute() {
+ boolean pressed = grab();
+
+ if (pressed) {
+ command.start();
+ } else if (m_pressedLast && !pressed) {
+ command.cancel();
+ }
+
+ m_pressedLast = pressed;
+ }
+ }.start();
+ }
+
+ /**
+ * Starts the command when the trigger becomes inactive.
+ *
+ * @param command the command to start
+ */
+ public void whenInactive(final Command command) {
+ new ButtonScheduler() {
+ private boolean m_pressedLast = grab();
+
+ @Override
+ public void execute() {
+ boolean pressed = grab();
+
+ if (m_pressedLast && !pressed) {
+ command.start();
+ }
+
+ m_pressedLast = pressed;
+ }
+ }.start();
+ }
+
+ /**
+ * Toggles a command when the trigger becomes active.
+ *
+ * @param command the command to toggle
+ */
+ public void toggleWhenActive(final Command command) {
+ new ButtonScheduler() {
+ private boolean m_pressedLast = grab();
+
+ @Override
+ public void execute() {
+ boolean pressed = grab();
+
+ if (!m_pressedLast && pressed) {
+ if (command.isRunning()) {
+ command.cancel();
+ } else {
+ command.start();
+ }
+ }
+
+ m_pressedLast = pressed;
+ }
+ }.start();
+ }
+
+ /**
+ * Cancels a command when the trigger becomes active.
+ *
+ * @param command the command to cancel
+ */
+ public void cancelWhenActive(final Command command) {
+ new ButtonScheduler() {
+ private boolean m_pressedLast = grab();
+
+ @Override
+ public void execute() {
+ boolean pressed = grab();
+
+ if (!m_pressedLast && pressed) {
+ command.cancel();
+ }
+
+ m_pressedLast = pressed;
+ }
+ }.start();
+ }
+
+ /**
+ * An internal class of {@link Trigger}. The user should ignore this, it is only public to
+ * interface between packages.
+ */
+ public abstract static class ButtonScheduler {
+ public abstract void execute();
+
+ public void start() {
+ Scheduler.getInstance().addButton(this);
+ }
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Button");
+ builder.setSafeState(() -> m_sendablePressed = false);
+ builder.addBooleanProperty("pressed", this::grab, value -> m_sendablePressed = value);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java
new file mode 100644
index 0000000..0033f58
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java
@@ -0,0 +1,626 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * The Command class is at the very core of the entire command framework. Every command can be
+ * started with a call to {@link Command#start() start()}. Once a command is started it will call
+ * {@link Command#initialize() initialize()}, and then will repeatedly call {@link Command#execute()
+ * execute()} until the {@link Command#isFinished() isFinished()} returns true. Once it does, {@link
+ * Command#end() end()} will be called.
+ *
+ * <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called,
+ * then the command will be stopped and {@link Command#interrupted() interrupted()} will be called.
+ *
+ * <p>If a command uses a {@link Subsystem}, then it should specify that it does so by calling the
+ * {@link Command#requires(Subsystem) requires(...)} method in its constructor. Note that a Command
+ * may have multiple requirements, and {@link Command#requires(Subsystem) requires(...)} should be
+ * called for each one.
+ *
+ * <p>If a command is running and a new command with shared requirements is started, then one of
+ * two things will happen. If the active command is interruptible, then {@link Command#cancel()
+ * cancel()} will be called and the command will be removed to make way for the new one. If the
+ * active command is not interruptible, the other one will not even be started, and the active one
+ * will continue functioning.
+ *
+ * @see Subsystem
+ * @see CommandGroup
+ * @see IllegalUseOfCommandException
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class Command extends SendableBase {
+ /**
+ * The time since this command was initialized.
+ */
+ private double m_startTime = -1;
+
+ /**
+ * The time (in seconds) before this command "times out" (or -1 if no timeout).
+ */
+ private double m_timeout = -1;
+
+ /**
+ * Whether or not this command has been initialized.
+ */
+ private boolean m_initialized;
+
+ /**
+ * The required subsystems.
+ */
+ private final Set m_requirements = new Set();
+
+ /**
+ * Whether or not it is running.
+ */
+ private boolean m_running;
+
+ /**
+ * Whether or not it is interruptible.
+ */
+ private boolean m_interruptible = true;
+
+ /**
+ * Whether or not it has been canceled.
+ */
+ private boolean m_canceled;
+
+ /**
+ * Whether or not it has been locked.
+ */
+ private boolean m_locked;
+
+ /**
+ * Whether this command should run when the robot is disabled.
+ */
+ private boolean m_runWhenDisabled;
+
+ /**
+ * Whether or not this command has completed running.
+ */
+ private boolean m_completed;
+
+ /**
+ * The {@link CommandGroup} this is in.
+ */
+ private CommandGroup m_parent;
+
+ /**
+ * Creates a new command. The name of this command will be set to its class name.
+ */
+ public Command() {
+ super(false);
+ String name = getClass().getName();
+ setName(name.substring(name.lastIndexOf('.') + 1));
+ }
+
+ /**
+ * Creates a new command with the given name.
+ *
+ * @param name the name for this command
+ * @throws IllegalArgumentException if name is null
+ */
+ public Command(String name) {
+ super(false);
+ if (name == null) {
+ throw new IllegalArgumentException("Name must not be null.");
+ }
+ setName(name);
+ }
+
+ /**
+ * Creates a new command with the given timeout and a default name. The default name is the name
+ * of the class.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ * @throws IllegalArgumentException if given a negative timeout
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ public Command(double timeout) {
+ this();
+ if (timeout < 0) {
+ throw new IllegalArgumentException("Timeout must not be negative. Given:" + timeout);
+ }
+ m_timeout = timeout;
+ }
+
+ /**
+ * Creates a new command with the given timeout and a default name. The default name is the name
+ * of the class.
+ *
+ * @param subsystem the subsystem that this command requires
+ * @throws IllegalArgumentException if given a negative timeout
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ public Command(Subsystem subsystem) {
+ this();
+ requires(subsystem);
+ }
+
+ /**
+ * Creates a new command with the given name.
+ *
+ * @param name the name for this command
+ * @param subsystem the subsystem that this command requires
+ * @throws IllegalArgumentException if name is null
+ */
+ public Command(String name, Subsystem subsystem) {
+ this(name);
+ requires(subsystem);
+ }
+
+ /**
+ * Creates a new command with the given timeout and a default name. The default name is the name
+ * of the class.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ * @param subsystem the subsystem that this command requires
+ * @throws IllegalArgumentException if given a negative timeout
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ public Command(double timeout, Subsystem subsystem) {
+ this(timeout);
+ requires(subsystem);
+ }
+
+ /**
+ * Creates a new command with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ * @throws IllegalArgumentException if given a negative timeout or name was null.
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ public Command(String name, double timeout) {
+ this(name);
+ if (timeout < 0) {
+ throw new IllegalArgumentException("Timeout must not be negative. Given:" + timeout);
+ }
+ m_timeout = timeout;
+ }
+
+ /**
+ * Creates a new command with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ * @param subsystem the subsystem that this command requires
+ * @throws IllegalArgumentException if given a negative timeout
+ * @throws IllegalArgumentException if given a negative timeout or name was null.
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ public Command(String name, double timeout, Subsystem subsystem) {
+ this(name, timeout);
+ requires(subsystem);
+ }
+
+ /**
+ * Sets the timeout of this command.
+ *
+ * @param seconds the timeout (in seconds)
+ * @throws IllegalArgumentException if seconds is negative
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ protected final synchronized void setTimeout(double seconds) {
+ if (seconds < 0) {
+ throw new IllegalArgumentException("Seconds must be positive. Given:" + seconds);
+ }
+ m_timeout = seconds;
+ }
+
+ /**
+ * Returns the time since this command was initialized (in seconds). This function will work even
+ * if there is no specified timeout.
+ *
+ * @return the time since this command was initialized (in seconds).
+ */
+ public final synchronized double timeSinceInitialized() {
+ return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
+ }
+
+ /**
+ * This method specifies that the given {@link Subsystem} is used by this command. This method is
+ * crucial to the functioning of the Command System in general.
+ *
+ * <p>Note that the recommended way to call this method is in the constructor.
+ *
+ * @param subsystem the {@link Subsystem} required
+ * @throws IllegalArgumentException if subsystem is null
+ * @throws IllegalUseOfCommandException if this command has started before or if it has been given
+ * to a {@link CommandGroup}
+ * @see Subsystem
+ */
+ protected synchronized void requires(Subsystem subsystem) {
+ validate("Can not add new requirement to command");
+ if (subsystem != null) {
+ m_requirements.add(subsystem);
+ } else {
+ throw new IllegalArgumentException("Subsystem must not be null.");
+ }
+ }
+
+ /**
+ * Called when the command has been removed. This will call {@link Command#interrupted()
+ * interrupted()} or {@link Command#end() end()}.
+ */
+ synchronized void removed() {
+ if (m_initialized) {
+ if (isCanceled()) {
+ interrupted();
+ _interrupted();
+ } else {
+ end();
+ _end();
+ }
+ }
+ m_initialized = false;
+ m_canceled = false;
+ m_running = false;
+ m_completed = true;
+ }
+
+ /**
+ * The run method is used internally to actually run the commands.
+ *
+ * @return whether or not the command should stay within the {@link Scheduler}.
+ */
+ synchronized boolean run() {
+ if (!m_runWhenDisabled && m_parent == null && RobotState.isDisabled()) {
+ cancel();
+ }
+ if (isCanceled()) {
+ return false;
+ }
+ if (!m_initialized) {
+ m_initialized = true;
+ startTiming();
+ _initialize();
+ initialize();
+ }
+ _execute();
+ execute();
+ return !isFinished();
+ }
+
+ /**
+ * The initialize method is called the first time this Command is run after being started.
+ */
+ protected void initialize() {}
+
+ /**
+ * A shadow method called before {@link Command#initialize() initialize()}.
+ */
+ @SuppressWarnings("MethodName")
+ void _initialize() {
+ }
+
+ /**
+ * The execute method is called repeatedly until this Command either finishes or is canceled.
+ */
+ @SuppressWarnings("MethodName")
+ protected void execute() {}
+
+ /**
+ * A shadow method called before {@link Command#execute() execute()}.
+ */
+ @SuppressWarnings("MethodName")
+ void _execute() {
+ }
+
+ /**
+ * Returns whether this command is finished. If it is, then the command will be removed and {@link
+ * Command#end() end()} will be called.
+ *
+ * <p>It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()}
+ * method for time-sensitive commands.
+ *
+ * <p>Returning false will result in the command never ending automatically. It may still be
+ * cancelled manually or interrupted by another command. Returning true will result in the
+ * command executing once and finishing immediately. We recommend using {@link InstantCommand}
+ * for this.
+ *
+ * @return whether this command is finished.
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ protected abstract boolean isFinished();
+
+ /**
+ * Called when the command ended peacefully. This is where you may want to wrap up loose ends,
+ * like shutting off a motor that was being used in the command.
+ */
+ protected void end() {}
+
+ /**
+ * A shadow method called after {@link Command#end() end()}.
+ */
+ @SuppressWarnings("MethodName")
+ void _end() {
+ }
+
+ /**
+ * Called when the command ends because somebody called {@link Command#cancel() cancel()} or
+ * another command shared the same requirements as this one, and booted it out.
+ *
+ * <p>This is where you may want to wrap up loose ends, like shutting off a motor that was being
+ * used in the command.
+ *
+ * <p>Generally, it is useful to simply call the {@link Command#end() end()} method within this
+ * method, as done here.
+ */
+ protected void interrupted() {
+ end();
+ }
+
+ /**
+ * A shadow method called after {@link Command#interrupted() interrupted()}.
+ */
+ @SuppressWarnings("MethodName")
+ void _interrupted() {}
+
+ /**
+ * Called to indicate that the timer should start. This is called right before {@link
+ * Command#initialize() initialize()} is, inside the {@link Command#run() run()} method.
+ */
+ private void startTiming() {
+ m_startTime = Timer.getFPGATimestamp();
+ }
+
+ /**
+ * Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()} method
+ * returns a number which is greater than or equal to the timeout for the command. If there is no
+ * timeout, this will always return false.
+ *
+ * @return whether the time has expired
+ */
+ protected synchronized boolean isTimedOut() {
+ return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
+ }
+
+ /**
+ * Returns the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
+ * Subsystems}) of this command.
+ *
+ * @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
+ * Subsystems}) of this command
+ */
+ synchronized Enumeration getRequirements() {
+ return m_requirements.getElements();
+ }
+
+ /**
+ * Prevents further changes from being made.
+ */
+ synchronized void lockChanges() {
+ m_locked = true;
+ }
+
+ /**
+ * If changes are locked, then this will throw an {@link IllegalUseOfCommandException}.
+ *
+ * @param message the message to say (it is appended by a default message)
+ */
+ synchronized void validate(String message) {
+ if (m_locked) {
+ throw new IllegalUseOfCommandException(message
+ + " after being started or being added to a command group");
+ }
+ }
+
+ /**
+ * Sets the parent of this command. No actual change is made to the group.
+ *
+ * @param parent the parent
+ * @throws IllegalUseOfCommandException if this {@link Command} already is already in a group
+ */
+ synchronized void setParent(CommandGroup parent) {
+ if (m_parent != null) {
+ throw new IllegalUseOfCommandException(
+ "Can not give command to a command group after already being put in a command group");
+ }
+ lockChanges();
+ m_parent = parent;
+ }
+
+ /**
+ * Returns whether the command has a parent.
+ *
+ * @return true if the command has a parent.
+ */
+ synchronized boolean isParented() {
+ return m_parent != null;
+ }
+
+ /**
+ * Clears list of subsystem requirements. This is only used by
+ * {@link ConditionalCommand} so cancelling the chosen command works properly
+ * in {@link CommandGroup}.
+ */
+ protected void clearRequirements() {
+ m_requirements.clear();
+ }
+
+ /**
+ * Starts up the command. Gets the command ready to start. <p> Note that the command will
+ * eventually start, however it will not necessarily do so immediately, and may in fact be
+ * canceled before initialize is even called. </p>
+ *
+ * @throws IllegalUseOfCommandException if the command is a part of a CommandGroup
+ */
+ public synchronized void start() {
+ lockChanges();
+ if (m_parent != null) {
+ throw new IllegalUseOfCommandException(
+ "Can not start a command that is a part of a command group");
+ }
+ Scheduler.getInstance().add(this);
+ m_completed = false;
+ }
+
+ /**
+ * This is used internally to mark that the command has been started. The lifecycle of a command
+ * is:
+ *
+ * <p>startRunning() is called. run() is called (multiple times potentially) removed() is called.
+ *
+ * <p>It is very important that startRunning and removed be called in order or some assumptions of
+ * the code will be broken.
+ */
+ synchronized void startRunning() {
+ m_running = true;
+ m_startTime = -1;
+ }
+
+ /**
+ * Returns whether or not the command is running. This may return true even if the command has
+ * just been canceled, as it may not have yet called {@link Command#interrupted()}.
+ *
+ * @return whether or not the command is running
+ */
+ public synchronized boolean isRunning() {
+ return m_running;
+ }
+
+ /**
+ * This will cancel the current command. <p> This will cancel the current command eventually. It
+ * can be called multiple times. And it can be called when the command is not running. If the
+ * command is running though, then the command will be marked as canceled and eventually removed.
+ * </p> <p> A command can not be canceled if it is a part of a command group, you must cancel the
+ * command group instead. </p>
+ *
+ * @throws IllegalUseOfCommandException if this command is a part of a command group
+ */
+ public synchronized void cancel() {
+ if (m_parent != null) {
+ throw new IllegalUseOfCommandException("Can not manually cancel a command in a command "
+ + "group");
+ }
+ _cancel();
+ }
+
+ /**
+ * This works like cancel(), except that it doesn't throw an exception if it is a part of a
+ * command group. Should only be called by the parent command group.
+ */
+ @SuppressWarnings("MethodName")
+ synchronized void _cancel() {
+ if (isRunning()) {
+ m_canceled = true;
+ }
+ }
+
+ /**
+ * Returns whether or not this has been canceled.
+ *
+ * @return whether or not this has been canceled
+ */
+ public synchronized boolean isCanceled() {
+ return m_canceled;
+ }
+
+ /**
+ * Whether or not this command has completed running.
+ *
+ * @return whether or not this command has completed running.
+ */
+ public synchronized boolean isCompleted() {
+ return m_completed;
+ }
+
+ /**
+ * Returns whether or not this command can be interrupted.
+ *
+ * @return whether or not this command can be interrupted
+ */
+ public synchronized boolean isInterruptible() {
+ return m_interruptible;
+ }
+
+ /**
+ * Sets whether or not this command can be interrupted.
+ *
+ * @param interruptible whether or not this command can be interrupted
+ */
+ protected synchronized void setInterruptible(boolean interruptible) {
+ m_interruptible = interruptible;
+ }
+
+ /**
+ * Checks if the command requires the given {@link Subsystem}.
+ *
+ * @param system the system
+ * @return whether or not the subsystem is required, or false if given null
+ */
+ public synchronized boolean doesRequire(Subsystem system) {
+ return m_requirements.contains(system);
+ }
+
+ /**
+ * Returns the {@link CommandGroup} that this command is a part of. Will return null if this
+ * {@link Command} is not in a group.
+ *
+ * @return the {@link CommandGroup} that this command is a part of (or null if not in group)
+ */
+ public synchronized CommandGroup getGroup() {
+ return m_parent;
+ }
+
+ /**
+ * Sets whether or not this {@link Command} should run when the robot is disabled.
+ *
+ * <p>By default a command will not run when the robot is disabled, and will in fact be canceled.
+ *
+ * @param run whether or not this command should run when the robot is disabled
+ */
+ public void setRunWhenDisabled(boolean run) {
+ m_runWhenDisabled = run;
+ }
+
+ /**
+ * Returns whether or not this {@link Command} will run when the robot is disabled, or if it will
+ * cancel itself.
+ *
+ * @return True if this command will run when the robot is disabled.
+ */
+ public boolean willRunWhenDisabled() {
+ return m_runWhenDisabled;
+ }
+
+ /**
+ * The string representation for a {@link Command} is by default its name.
+ *
+ * @return the string representation of this object
+ */
+ @Override
+ public String toString() {
+ return getName();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Command");
+ builder.addStringProperty(".name", this::getName, null);
+ builder.addBooleanProperty("running", this::isRunning, value -> {
+ if (value) {
+ if (!isRunning()) {
+ start();
+ }
+ } else {
+ if (isRunning()) {
+ cancel();
+ }
+ }
+ });
+ builder.addBooleanProperty(".isParented", this::isParented, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
new file mode 100644
index 0000000..53b62ee
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
@@ -0,0 +1,422 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+import java.util.Vector;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * A {@link CommandGroup} is a list of commands which are executed in sequence.
+ *
+ * <p> Commands in a {@link CommandGroup} are added using the {@link
+ * CommandGroup#addSequential(Command) addSequential(...)} method and are called sequentially.
+ * {@link CommandGroup CommandGroups} are themselves {@link Command commands} and can be given to
+ * other {@link CommandGroup CommandGroups}. </p>
+ *
+ * <p> {@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command
+ * subcommands}. Additional requirements can be specified by calling {@link
+ * CommandGroup#requires(Subsystem) requires(...)} normally in the constructor. </P>
+ *
+ * <p> CommandGroups can also execute commands in parallel, simply by adding them using {@link
+ * CommandGroup#addParallel(Command) addParallel(...)}. </p>
+ *
+ * @see Command
+ * @see Subsystem
+ * @see IllegalUseOfCommandException
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class CommandGroup extends Command {
+ /**
+ * The commands in this group (stored in entries).
+ */
+ @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+ private final Vector<Entry> m_commands = new Vector<>();
+ /*
+ * Intentionally package private
+ */
+ /**
+ * The active children in this group (stored in entries).
+ */
+ @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+ final Vector<Entry> m_children = new Vector<>();
+ /**
+ * The current command, -1 signifies that none have been run.
+ */
+ private int m_currentCommandIndex = -1;
+
+ /**
+ * Creates a new {@link CommandGroup CommandGroup}. The name of this command will be set to its
+ * class name.
+ */
+ public CommandGroup() {
+ }
+
+ /**
+ * Creates a new {@link CommandGroup CommandGroup} with the given name.
+ *
+ * @param name the name for this command group
+ * @throws IllegalArgumentException if name is null
+ */
+ public CommandGroup(String name) {
+ super(name);
+ }
+
+ /**
+ * Adds a new {@link Command Command} to the group. The {@link Command Command} will be started
+ * after all the previously added {@link Command Commands}.
+ *
+ * <p> Note that any requirements the given {@link Command Command} has will be added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be changed after being
+ * added to a group. </p>
+ *
+ * <p> It is recommended that this method be called in the constructor. </p>
+ *
+ * @param command The {@link Command Command} to be added
+ * @throws IllegalUseOfCommandException if the group has been started before or been given to
+ * another group
+ * @throws IllegalArgumentException if command is null
+ */
+ public final synchronized void addSequential(Command command) {
+ validate("Can not add new command to command group");
+ if (command == null) {
+ throw new IllegalArgumentException("Given null command");
+ }
+
+ command.setParent(this);
+
+ m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
+ for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+ requires((Subsystem) e.nextElement());
+ }
+ }
+
+ /**
+ * Adds a new {@link Command Command} to the group with a given timeout. The {@link Command
+ * Command} will be started after all the previously added commands.
+ *
+ * <p> Once the {@link Command Command} is started, it will be run until it finishes or the time
+ * expires, whichever is sooner. Note that the given {@link Command Command} will have no
+ * knowledge that it is on a timer. </p>
+ *
+ * <p> Note that any requirements the given {@link Command Command} has will be added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be changed after being
+ * added to a group. </p>
+ *
+ * <p> It is recommended that this method be called in the constructor. </p>
+ *
+ * @param command The {@link Command Command} to be added
+ * @param timeout The timeout (in seconds)
+ * @throws IllegalUseOfCommandException if the group has been started before or been given to
+ * another group or if the {@link Command Command} has been
+ * started before or been given to another group
+ * @throws IllegalArgumentException if command is null or timeout is negative
+ */
+ public final synchronized void addSequential(Command command, double timeout) {
+ validate("Can not add new command to command group");
+ if (command == null) {
+ throw new IllegalArgumentException("Given null command");
+ }
+ if (timeout < 0) {
+ throw new IllegalArgumentException("Can not be given a negative timeout");
+ }
+
+ command.setParent(this);
+
+ m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
+ for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+ requires((Subsystem) e.nextElement());
+ }
+ }
+
+ /**
+ * Adds a new child {@link Command} to the group. The {@link Command} will be started after all
+ * the previously added {@link Command Commands}.
+ *
+ * <p> Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
+ * same time as the subsequent {@link Command Commands}. The child will run until either it
+ * finishes, a new child with conflicting requirements is started, or the main sequence runs a
+ * {@link Command} with conflicting requirements. In the latter two cases, the child will be
+ * canceled even if it says it can't be interrupted. </p>
+ *
+ * <p> Note that any requirements the given {@link Command Command} has will be added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be changed after being
+ * added to a group. </p>
+ *
+ * <p> It is recommended that this method be called in the constructor. </p>
+ *
+ * @param command The command to be added
+ * @throws IllegalUseOfCommandException if the group has been started before or been given to
+ * another command group
+ * @throws IllegalArgumentException if command is null
+ */
+ public final synchronized void addParallel(Command command) {
+ requireNonNull(command, "Provided command was null");
+ validate("Can not add new command to command group");
+
+ command.setParent(this);
+
+ m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
+ for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+ requires((Subsystem) e.nextElement());
+ }
+ }
+
+ /**
+ * Adds a new child {@link Command} to the group with the given timeout. The {@link Command} will
+ * be started after all the previously added {@link Command Commands}.
+ *
+ * <p> Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
+ * or the time expires, whichever is sooner. Note that the given {@link Command Command} will have
+ * no knowledge that it is on a timer. </p>
+ *
+ * <p> Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
+ * same time as the subsequent {@link Command Commands}. The child will run until either it
+ * finishes, the timeout expires, a new child with conflicting requirements is started, or the
+ * main sequence runs a {@link Command} with conflicting requirements. In the latter two cases,
+ * the child will be canceled even if it says it can't be interrupted. </p>
+ *
+ * <p> Note that any requirements the given {@link Command Command} has will be added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be changed after being
+ * added to a group. </p>
+ *
+ * <p> It is recommended that this method be called in the constructor. </p>
+ *
+ * @param command The command to be added
+ * @param timeout The timeout (in seconds)
+ * @throws IllegalUseOfCommandException if the group has been started before or been given to
+ * another command group
+ * @throws IllegalArgumentException if command is null
+ */
+ public final synchronized void addParallel(Command command, double timeout) {
+ requireNonNull(command, "Provided command was null");
+ if (timeout < 0) {
+ throw new IllegalArgumentException("Can not be given a negative timeout");
+ }
+ validate("Can not add new command to command group");
+
+ command.setParent(this);
+
+ m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
+ for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+ requires((Subsystem) e.nextElement());
+ }
+ }
+
+ @Override
+ @SuppressWarnings("MethodName")
+ void _initialize() {
+ m_currentCommandIndex = -1;
+ }
+
+ @Override
+ @SuppressWarnings({"MethodName", "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ void _execute() {
+ Entry entry = null;
+ Command cmd = null;
+ boolean firstRun = false;
+ if (m_currentCommandIndex == -1) {
+ firstRun = true;
+ m_currentCommandIndex = 0;
+ }
+
+ while (m_currentCommandIndex < m_commands.size()) {
+ if (cmd != null) {
+ if (entry.isTimedOut()) {
+ cmd._cancel();
+ }
+ if (cmd.run()) {
+ break;
+ } else {
+ cmd.removed();
+ m_currentCommandIndex++;
+ firstRun = true;
+ cmd = null;
+ continue;
+ }
+ }
+
+ entry = m_commands.elementAt(m_currentCommandIndex);
+ cmd = null;
+
+ switch (entry.m_state) {
+ case Entry.IN_SEQUENCE:
+ cmd = entry.m_command;
+ if (firstRun) {
+ cmd.startRunning();
+ cancelConflicts(cmd);
+ }
+ firstRun = false;
+ break;
+ case Entry.BRANCH_PEER:
+ m_currentCommandIndex++;
+ entry.m_command.start();
+ break;
+ case Entry.BRANCH_CHILD:
+ m_currentCommandIndex++;
+ cancelConflicts(entry.m_command);
+ entry.m_command.startRunning();
+ m_children.addElement(entry);
+ break;
+ default:
+ break;
+ }
+ }
+
+ // Run Children
+ for (int i = 0; i < m_children.size(); i++) {
+ entry = m_children.elementAt(i);
+ Command child = entry.m_command;
+ if (entry.isTimedOut()) {
+ child._cancel();
+ }
+ if (!child.run()) {
+ child.removed();
+ m_children.removeElementAt(i--);
+ }
+ }
+ }
+
+ @Override
+ @SuppressWarnings("MethodName")
+ void _end() {
+ // Theoretically, we don't have to check this, but we do if teams override
+ // the isFinished method
+ if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
+ Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
+ cmd._cancel();
+ cmd.removed();
+ }
+
+ Enumeration children = m_children.elements();
+ while (children.hasMoreElements()) {
+ Command cmd = ((Entry) children.nextElement()).m_command;
+ cmd._cancel();
+ cmd.removed();
+ }
+ m_children.removeAllElements();
+ }
+
+ @Override
+ @SuppressWarnings("MethodName")
+ void _interrupted() {
+ _end();
+ }
+
+ /**
+ * Returns true if all the {@link Command Commands} in this group have been started and have
+ * finished.
+ *
+ * <p> Teams may override this method, although they should probably reference super.isFinished()
+ * if they do. </p>
+ *
+ * @return whether this {@link CommandGroup} is finished
+ */
+ @Override
+ protected boolean isFinished() {
+ return m_currentCommandIndex >= m_commands.size() && m_children.isEmpty();
+ }
+
+ // Can be overwritten by teams
+ @Override
+ protected void initialize() {
+ }
+
+ // Can be overwritten by teams
+ @Override
+ protected void execute() {
+ }
+
+ // Can be overwritten by teams
+ @Override
+ protected void end() {
+ }
+
+ // Can be overwritten by teams
+ @Override
+ protected void interrupted() {
+ }
+
+ /**
+ * Returns whether or not this group is interruptible. A command group will be uninterruptible if
+ * {@link CommandGroup#setInterruptible(boolean) setInterruptable(false)} was called or if it is
+ * currently running an uninterruptible command or child.
+ *
+ * @return whether or not this {@link CommandGroup} is interruptible.
+ */
+ @Override
+ public synchronized boolean isInterruptible() {
+ if (!super.isInterruptible()) {
+ return false;
+ }
+
+ if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
+ Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
+ if (!cmd.isInterruptible()) {
+ return false;
+ }
+ }
+
+ for (int i = 0; i < m_children.size(); i++) {
+ if (!m_children.elementAt(i).m_command.isInterruptible()) {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ private void cancelConflicts(Command command) {
+ for (int i = 0; i < m_children.size(); i++) {
+ Command child = m_children.elementAt(i).m_command;
+
+ Enumeration requirements = command.getRequirements();
+
+ while (requirements.hasMoreElements()) {
+ Object requirement = requirements.nextElement();
+ if (child.doesRequire((Subsystem) requirement)) {
+ child._cancel();
+ child.removed();
+ m_children.removeElementAt(i--);
+ break;
+ }
+ }
+ }
+ }
+
+ private static class Entry {
+ private static final int IN_SEQUENCE = 0;
+ private static final int BRANCH_PEER = 1;
+ private static final int BRANCH_CHILD = 2;
+ private final Command m_command;
+ private final int m_state;
+ private final double m_timeout;
+
+ Entry(Command command, int state) {
+ m_command = command;
+ m_state = state;
+ m_timeout = -1;
+ }
+
+ Entry(Command command, int state, double timeout) {
+ m_command = command;
+ m_state = state;
+ m_timeout = timeout;
+ }
+
+ boolean isTimedOut() {
+ if (m_timeout == -1) {
+ return false;
+ } else {
+ double time = m_command.timeSinceInitialized();
+ return time != 0 && time >= m_timeout;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
new file mode 100644
index 0000000..626f744
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+
+/**
+ * A {@link ConditionalCommand} is a {@link Command} that starts one of two commands.
+ *
+ * <p>
+ * A {@link ConditionalCommand} uses m_condition to determine whether it should run m_onTrue or
+ * m_onFalse.
+ * </p>
+ *
+ * <p>
+ * A {@link ConditionalCommand} adds the proper {@link Command} to the {@link Scheduler} during
+ * {@link ConditionalCommand#initialize()} and then {@link ConditionalCommand#isFinished()} will
+ * return true once that {@link Command} has finished executing.
+ * </p>
+ *
+ * <p>
+ * If no {@link Command} is specified for m_onFalse, the occurrence of that condition will be a
+ * no-op.
+ * </p>
+ *
+ * @see Command
+ * @see Scheduler
+ */
+public abstract class ConditionalCommand extends Command {
+ /**
+ * The Command to execute if {@link ConditionalCommand#condition()} returns true.
+ */
+ private Command m_onTrue;
+
+ /**
+ * The Command to execute if {@link ConditionalCommand#condition()} returns false.
+ */
+ private Command m_onFalse;
+
+ /**
+ * Stores command chosen by condition.
+ */
+ private Command m_chosenCommand;
+
+ private void requireAll() {
+ if (m_onTrue != null) {
+ for (Enumeration e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
+ requires((Subsystem) e.nextElement());
+ }
+ }
+
+ if (m_onFalse != null) {
+ for (Enumeration e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
+ requires((Subsystem) e.nextElement());
+ }
+ }
+ }
+
+ /**
+ * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+ *
+ * <p>Users of this constructor should also override condition().
+ *
+ * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
+ */
+ public ConditionalCommand(Command onTrue) {
+ this(onTrue, null);
+ }
+
+ /**
+ * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+ *
+ * <p>Users of this constructor should also override condition().
+ *
+ * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
+ * @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false
+ */
+ public ConditionalCommand(Command onTrue, Command onFalse) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ requireAll();
+ }
+
+ /**
+ * Creates a new ConditionalCommand with given name and onTrue and onFalse Commands.
+ *
+ * <p>Users of this constructor should also override condition().
+ *
+ * @param name the name for this command group
+ * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
+ */
+ public ConditionalCommand(String name, Command onTrue) {
+ this(name, onTrue, null);
+ }
+
+ /**
+ * Creates a new ConditionalCommand with given name and onTrue and onFalse Commands.
+ *
+ * <p>Users of this constructor should also override condition().
+ *
+ * @param name the name for this command group
+ * @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
+ * @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false
+ */
+ public ConditionalCommand(String name, Command onTrue, Command onFalse) {
+ super(name);
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ requireAll();
+ }
+
+ /**
+ * The Condition to test to determine which Command to run.
+ *
+ * @return true if m_onTrue should be run or false if m_onFalse should be run.
+ */
+ protected abstract boolean condition();
+
+ /**
+ * Calls {@link ConditionalCommand#condition()} and runs the proper command.
+ */
+ @Override
+ protected void _initialize() {
+ if (condition()) {
+ m_chosenCommand = m_onTrue;
+ } else {
+ m_chosenCommand = m_onFalse;
+ }
+
+ if (m_chosenCommand != null) {
+ /*
+ * This is a hack to make cancelling the chosen command inside a
+ * CommandGroup work properly
+ */
+ m_chosenCommand.clearRequirements();
+
+ m_chosenCommand.start();
+ }
+ super._initialize();
+ }
+
+ @Override
+ protected synchronized void _cancel() {
+ if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
+ m_chosenCommand.cancel();
+ }
+
+ super._cancel();
+ }
+
+ @Override
+ protected boolean isFinished() {
+ if (m_chosenCommand != null) {
+ return m_chosenCommand.isCompleted();
+ } else {
+ return true;
+ }
+ }
+
+ @Override
+ protected void _interrupted() {
+ if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
+ m_chosenCommand.cancel();
+ }
+
+ super._interrupted();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
new file mode 100644
index 0000000..59f6e9c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * This exception will be thrown if a command is used illegally. There are several ways for this to
+ * happen.
+ *
+ * <p> Basically, a command becomes "locked" after it is first started or added to a command group.
+ * </p>
+ *
+ * <p> This exception should be thrown if (after a command has been locked) its requirements change,
+ * it is put into multiple command groups, it is started from outside its command group, or it adds
+ * a new child. </p>
+ */
+public class IllegalUseOfCommandException extends RuntimeException {
+ /**
+ * Instantiates an {@link IllegalUseOfCommandException}.
+ */
+ public IllegalUseOfCommandException() {
+ }
+
+ /**
+ * Instantiates an {@link IllegalUseOfCommandException} with the given message.
+ *
+ * @param message the message
+ */
+ public IllegalUseOfCommandException(String message) {
+ super(message);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
new file mode 100644
index 0000000..1f3c5de
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * This command will execute once, then finish immediately afterward.
+ *
+ * <p>Subclassing {@link InstantCommand} is shorthand for returning true from
+ * {@link Command isFinished}.
+ */
+public class InstantCommand extends Command {
+ private Runnable m_func;
+
+ public InstantCommand() {
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand} with the given name.
+ *
+ * @param name the name for this command
+ */
+ public InstantCommand(String name) {
+ super(name);
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand} with the given requirement.
+ *
+ * @param subsystem the subsystem this command requires
+ */
+ public InstantCommand(Subsystem subsystem) {
+ super(subsystem);
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand} with the given name and requirement.
+ *
+ * @param name the name for this command
+ * @param subsystem the subsystem this command requires
+ */
+ public InstantCommand(String name, Subsystem subsystem) {
+ super(name, subsystem);
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand}.
+ *
+ * @param func the function to run when {@link Command#initialize() initialize()} is run
+ */
+ public InstantCommand(Runnable func) {
+ m_func = func;
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand}.
+ *
+ * @param name the name for this command
+ * @param func the function to run when {@link Command#initialize() initialize()} is run
+ */
+ public InstantCommand(String name, Runnable func) {
+ super(name);
+ m_func = func;
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand}.
+ *
+ * @param requirement the subsystem this command requires
+ * @param func the function to run when {@link Command#initialize() initialize()} is run
+ */
+ public InstantCommand(Subsystem requirement, Runnable func) {
+ super(requirement);
+ m_func = func;
+ }
+
+ /**
+ * Creates a new {@link InstantCommand InstantCommand}.
+ *
+ * @param name the name for this command
+ * @param requirement the subsystem this command requires
+ * @param func the function to run when {@link Command#initialize() initialize()} is run
+ */
+ public InstantCommand(String name, Subsystem requirement, Runnable func) {
+ super(name, requirement);
+ m_func = func;
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
+
+ /**
+ * Trigger the stored function.
+ *
+ * <p>Called just before this Command runs the first time.
+ */
+ @Override
+ protected void _initialize() {
+ super._initialize();
+ if (m_func != null) {
+ m_func.run();
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
new file mode 100644
index 0000000..e717a47
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * An element that is in a LinkedList.
+ */
+class LinkedListElement {
+ private LinkedListElement m_next;
+ private LinkedListElement m_previous;
+ private Command m_data;
+
+ public void setData(Command newData) {
+ m_data = newData;
+ }
+
+ public Command getData() {
+ return m_data;
+ }
+
+ public LinkedListElement getNext() {
+ return m_next;
+ }
+
+ public LinkedListElement getPrevious() {
+ return m_previous;
+ }
+
+ public void add(LinkedListElement listElement) {
+ if (m_next == null) {
+ m_next = listElement;
+ m_next.m_previous = this;
+ } else {
+ m_next.m_previous = listElement;
+ listElement.m_next = m_next;
+ listElement.m_previous = this;
+ m_next = listElement;
+ }
+ }
+
+ @SuppressWarnings("PMD.EmptyIfStmt")
+ public LinkedListElement remove() {
+ if (m_previous == null && m_next == null) {
+ // no-op
+ } else if (m_next == null) {
+ m_previous.m_next = null;
+ } else if (m_previous == null) {
+ m_next.m_previous = null;
+ } else {
+ m_next.m_previous = m_previous;
+ m_previous.m_next = m_next;
+ }
+ LinkedListElement returnNext = m_next;
+ m_next = null;
+ m_previous = null;
+ return returnNext;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
new file mode 100644
index 0000000..3d7fd98
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
@@ -0,0 +1,284 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * This class defines a {@link Command} which interacts heavily with a PID loop.
+ *
+ * <p> It provides some convenience methods to run an internal {@link PIDController} . It will also
+ * start and stop said {@link PIDController} when the {@link PIDCommand} is first initialized and
+ * ended/interrupted. </p>
+ */
+public abstract class PIDCommand extends Command {
+ /**
+ * The internal {@link PIDController}.
+ */
+ private final PIDController m_controller;
+ /**
+ * An output which calls {@link PIDCommand#usePIDOutput(double)}.
+ */
+ private final PIDOutput m_output = this::usePIDOutput;
+ /**
+ * A source which calls {@link PIDCommand#returnPIDInput()}.
+ */
+ private final PIDSource m_source = new PIDSource() {
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return PIDSourceType.kDisplacement;
+ }
+
+ @Override
+ public double pidGet() {
+ return returnPIDInput();
+ }
+ };
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
+ *
+ * @param name the name of the command
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(String name, double p, double i, double d) {
+ super(name);
+ m_controller = new PIDController(p, i, d, m_source, m_output);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
+ * the time between PID loop calculations to be equal to the given period.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param period the time (in seconds) between calculations
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(String name, double p, double i, double d, double period) {
+ super(name);
+ m_controller = new PIDController(p, i, d, m_source, m_output, period);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
+ * class name as its name.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(double p, double i, double d) {
+ m_controller = new PIDController(p, i, d, m_source, m_output);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
+ * class name as its name. It will also space the time between PID loop calculations to be equal
+ * to the given period.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param period the time (in seconds) between calculations
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(double p, double i, double d, double period) {
+ m_controller = new PIDController(p, i, d, m_source, m_output, period);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
+ *
+ * @param name the name of the command
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param subsystem the subsystem that this command requires
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(String name, double p, double i, double d, Subsystem subsystem) {
+ super(name, subsystem);
+ m_controller = new PIDController(p, i, d, m_source, m_output);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
+ * the time between PID loop calculations to be equal to the given period.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param period the time (in seconds) between calculations
+ * @param subsystem the subsystem that this command requires
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(String name, double p, double i, double d, double period,
+ Subsystem subsystem) {
+ super(name, subsystem);
+ m_controller = new PIDController(p, i, d, m_source, m_output, period);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
+ * class name as its name.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param subsystem the subsystem that this command requires
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(double p, double i, double d, Subsystem subsystem) {
+ super(subsystem);
+ m_controller = new PIDController(p, i, d, m_source, m_output);
+ }
+
+ /**
+ * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
+ * class name as its name. It will also space the time between PID loop calculations to be equal
+ * to the given period.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param period the time (in seconds) between calculations
+ * @param subsystem the subsystem that this command requires
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDCommand(double p, double i, double d, double period, Subsystem subsystem) {
+ super(subsystem);
+ m_controller = new PIDController(p, i, d, m_source, m_output, period);
+ }
+
+ /**
+ * Returns the {@link PIDController} used by this {@link PIDCommand}. Use this if you would like
+ * to fine tune the pid loop.
+ *
+ * @return the {@link PIDController} used by this {@link PIDCommand}
+ */
+ protected PIDController getPIDController() {
+ return m_controller;
+ }
+
+ @Override
+ @SuppressWarnings("MethodName")
+ void _initialize() {
+ m_controller.enable();
+ }
+
+ @Override
+ @SuppressWarnings("MethodName")
+ void _end() {
+ m_controller.disable();
+ }
+
+ @Override
+ @SuppressWarnings("MethodName")
+ void _interrupted() {
+ _end();
+ }
+
+ /**
+ * Adds the given value to the setpoint. If {@link PIDCommand#setInputRange(double, double)
+ * setInputRange(...)} was used, then the bounds will still be honored by this method.
+ *
+ * @param deltaSetpoint the change in the setpoint
+ */
+ public void setSetpointRelative(double deltaSetpoint) {
+ setSetpoint(getSetpoint() + deltaSetpoint);
+ }
+
+ /**
+ * Sets the setpoint to the given value. If {@link PIDCommand#setInputRange(double, double)
+ * setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
+ * range.
+ *
+ * @param setpoint the new setpoint
+ */
+ protected void setSetpoint(double setpoint) {
+ m_controller.setSetpoint(setpoint);
+ }
+
+ /**
+ * Returns the setpoint.
+ *
+ * @return the setpoint
+ */
+ protected double getSetpoint() {
+ return m_controller.getSetpoint();
+ }
+
+ /**
+ * Returns the current position.
+ *
+ * @return the current position
+ */
+ protected double getPosition() {
+ return returnPIDInput();
+ }
+
+ /**
+ * Sets the maximum and minimum values expected from the input and setpoint.
+ *
+ * @param minimumInput the minimum value expected from the input and setpoint
+ * @param maximumInput the maximum value expected from the input and setpoint
+ */
+ protected void setInputRange(double minimumInput, double maximumInput) {
+ m_controller.setInputRange(minimumInput, maximumInput);
+ }
+
+ /**
+ * Returns the input for the pid loop.
+ *
+ * <p>It returns the input for the pid loop, so if this command was based off of a gyro, then it
+ * should return the angle of the gyro.
+ *
+ * <p>All subclasses of {@link PIDCommand} must override this method.
+ *
+ * <p>This method will be called in a different thread then the {@link Scheduler} thread.
+ *
+ * @return the value the pid loop should use as input
+ */
+ protected abstract double returnPIDInput();
+
+ /**
+ * Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
+ * This method is a good time to set motor values, maybe something along the lines of
+ * <code>driveline.tankDrive(output, -output)</code>
+ *
+ * <p>All subclasses of {@link PIDCommand} must override this method.
+ *
+ * <p>This method will be called in a different thread then the {@link Scheduler} thread.
+ *
+ * @param output the value the pid loop calculated
+ */
+ protected abstract void usePIDOutput(double output);
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ m_controller.initSendable(builder);
+ super.initSendable(builder);
+ builder.setSmartDashboardType("PIDCommand");
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
new file mode 100644
index 0000000..6465eb1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
@@ -0,0 +1,287 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/**
+ * This class is designed to handle the case where there is a {@link Subsystem} which uses a single
+ * {@link PIDController} almost constantly (for instance, an elevator which attempts to stay at a
+ * constant height).
+ *
+ * <p>It provides some convenience methods to run an internal {@link PIDController} . It also
+ * allows access to the internal {@link PIDController} in order to give total control to the
+ * programmer.
+ */
+public abstract class PIDSubsystem extends Subsystem {
+ /**
+ * The internal {@link PIDController}.
+ */
+ private final PIDController m_controller;
+ /**
+ * An output which calls {@link PIDCommand#usePIDOutput(double)}.
+ */
+ private final PIDOutput m_output = this::usePIDOutput;
+
+ /**
+ * A source which calls {@link PIDCommand#returnPIDInput()}.
+ */
+ private final PIDSource m_source = new PIDSource() {
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return PIDSourceType.kDisplacement;
+ }
+
+ @Override
+ public double pidGet() {
+ return returnPIDInput();
+ }
+ };
+
+ /**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDSubsystem(String name, double p, double i, double d) {
+ super(name);
+ m_controller = new PIDController(p, i, d, m_source, m_output);
+ addChild("PIDController", m_controller);
+ }
+
+ /**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feed forward value
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDSubsystem(String name, double p, double i, double d, double f) {
+ super(name);
+ m_controller = new PIDController(p, i, d, f, m_source, m_output);
+ addChild("PIDController", m_controller);
+ }
+
+ /**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will also
+ * space the time between PID loop calculations to be equal to the given period.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feed forward value
+ * @param period the time (in seconds) between calculations
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDSubsystem(String name, double p, double i, double d, double f, double period) {
+ super(name);
+ m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
+ addChild("PIDController", m_controller);
+ }
+
+ /**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
+ * class name as its name.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDSubsystem(double p, double i, double d) {
+ m_controller = new PIDController(p, i, d, m_source, m_output);
+ addChild("PIDController", m_controller);
+ }
+
+ /**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
+ * class name as its name. It will also space the time between PID loop calculations to be equal
+ * to the given period.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feed forward coefficient
+ * @param period the time (in seconds) between calculations
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDSubsystem(double p, double i, double d, double f, double period) {
+ m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
+ addChild("PIDController", m_controller);
+ }
+
+ /**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
+ * class name as its name. It will also space the time between PID loop calculations to be equal
+ * to the given period.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param period the time (in seconds) between calculations
+ */
+ @SuppressWarnings("ParameterName")
+ public PIDSubsystem(double p, double i, double d, double period) {
+ m_controller = new PIDController(p, i, d, m_source, m_output, period);
+ addChild("PIDController", m_controller);
+ }
+
+ /**
+ * Returns the {@link PIDController} used by this {@link PIDSubsystem}. Use this if you would like
+ * to fine tune the pid loop.
+ *
+ * @return the {@link PIDController} used by this {@link PIDSubsystem}
+ */
+ public PIDController getPIDController() {
+ return m_controller;
+ }
+
+
+ /**
+ * Adds the given value to the setpoint. If {@link PIDSubsystem#setInputRange(double, double)
+ * setInputRange(...)} was used, then the bounds will still be honored by this method.
+ *
+ * @param deltaSetpoint the change in the setpoint
+ */
+ public void setSetpointRelative(double deltaSetpoint) {
+ setSetpoint(getPosition() + deltaSetpoint);
+ }
+
+ /**
+ * Sets the setpoint to the given value. If {@link PIDSubsystem#setInputRange(double, double)
+ * setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
+ * range.
+ *
+ * @param setpoint the new setpoint
+ */
+ public void setSetpoint(double setpoint) {
+ m_controller.setSetpoint(setpoint);
+ }
+
+ /**
+ * Returns the setpoint.
+ *
+ * @return the setpoint
+ */
+ public double getSetpoint() {
+ return m_controller.getSetpoint();
+ }
+
+ /**
+ * Returns the current position.
+ *
+ * @return the current position
+ */
+ public double getPosition() {
+ return returnPIDInput();
+ }
+
+ /**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+ public void setInputRange(double minimumInput, double maximumInput) {
+ m_controller.setInputRange(minimumInput, maximumInput);
+ }
+
+ /**
+ * Sets the maximum and minimum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+ public void setOutputRange(double minimumOutput, double maximumOutput) {
+ m_controller.setOutputRange(minimumOutput, maximumOutput);
+ }
+
+ /**
+ * Set the absolute error which is considered tolerable for use with OnTarget. The value is in the
+ * same range as the PIDInput values.
+ *
+ * @param t the absolute tolerance
+ */
+ @SuppressWarnings("ParameterName")
+ public void setAbsoluteTolerance(double t) {
+ m_controller.setAbsoluteTolerance(t);
+ }
+
+ /**
+ * Set the percentage error which is considered tolerable for use with OnTarget. (Value of 15.0 ==
+ * 15 percent).
+ *
+ * @param p the percent tolerance
+ */
+ @SuppressWarnings("ParameterName")
+ public void setPercentTolerance(double p) {
+ m_controller.setPercentTolerance(p);
+ }
+
+ /**
+ * Return true if the error is within the percentage of the total input range, determined by
+ * setTolerance. This assumes that the maximum and minimum input were set using setInput.
+ *
+ * @return true if the error is less than the tolerance
+ */
+ public boolean onTarget() {
+ return m_controller.onTarget();
+ }
+
+ /**
+ * Returns the input for the pid loop.
+ *
+ * <p>It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then
+ * it should return the angle of the gyro.
+ *
+ * <p>All subclasses of {@link PIDSubsystem} must override this method.
+ *
+ * @return the value the pid loop should use as input
+ */
+ protected abstract double returnPIDInput();
+
+ /**
+ * Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
+ * This method is a good time to set motor values, maybe something along the lines of
+ * <code>driveline.tankDrive(output, -output)</code>.
+ *
+ * <p>All subclasses of {@link PIDSubsystem} must override this method.
+ *
+ * @param output the value the pid loop calculated
+ */
+ protected abstract void usePIDOutput(double output);
+
+ /**
+ * Enables the internal {@link PIDController}.
+ */
+ public void enable() {
+ m_controller.enable();
+ }
+
+ /**
+ * Disables the internal {@link PIDController}.
+ */
+ public void disable() {
+ m_controller.disable();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
new file mode 100644
index 0000000..6199c6d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link PrintCommand} is a command which prints out a string when it is initialized, and then
+ * immediately finishes. It is useful if you want a {@link CommandGroup} to print out a string when
+ * it reaches a certain point.
+ */
+public class PrintCommand extends InstantCommand {
+ /**
+ * The message to print out.
+ */
+ private final String m_message;
+
+ /**
+ * Instantiates a {@link PrintCommand} which will print the given message when it is run.
+ *
+ * @param message the message to print
+ */
+ public PrintCommand(String message) {
+ super("Print(\"" + message + "\"");
+ m_message = message;
+ }
+
+ @Override
+ protected void initialize() {
+ System.out.println(m_message);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
new file mode 100644
index 0000000..8302e22
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+import java.util.Hashtable;
+import java.util.Vector;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * The {@link Scheduler} is a singleton which holds the top-level running commands. It is in charge
+ * of both calling the command's {@link Command#run() run()} method and to make sure that there are
+ * no two commands with conflicting requirements running.
+ *
+ * <p> It is fine if teams wish to take control of the {@link Scheduler} themselves, all that needs
+ * to be done is to call {@link Scheduler#getInstance() Scheduler.getInstance()}.{@link
+ * Scheduler#run() run()} often to have {@link Command Commands} function correctly. However, this
+ * is already done for you if you use the CommandBased Robot template. </p>
+ *
+ * @see Command
+ */
+public final class Scheduler extends SendableBase {
+ /**
+ * The Singleton Instance.
+ */
+ private static Scheduler instance;
+
+ /**
+ * Returns the {@link Scheduler}, creating it if one does not exist.
+ *
+ * @return the {@link Scheduler}
+ */
+ public static synchronized Scheduler getInstance() {
+ if (instance == null) {
+ instance = new Scheduler();
+ }
+ return instance;
+ }
+
+ /**
+ * A hashtable of active {@link Command Commands} to their {@link LinkedListElement}.
+ */
+ @SuppressWarnings("PMD.LooseCoupling")
+ private final Hashtable<Command, LinkedListElement> m_commandTable = new Hashtable<>();
+ /**
+ * The {@link Set} of all {@link Subsystem Subsystems}.
+ */
+ private final Set m_subsystems = new Set();
+ /**
+ * The first {@link Command} in the list.
+ */
+ private LinkedListElement m_firstCommand;
+ /**
+ * The last {@link Command} in the list.
+ */
+ private LinkedListElement m_lastCommand;
+ /**
+ * Whether or not we are currently adding a command.
+ */
+ private boolean m_adding;
+ /**
+ * Whether or not we are currently disabled.
+ */
+ private boolean m_disabled;
+ /**
+ * A list of all {@link Command Commands} which need to be added.
+ */
+ @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+ private final Vector<Command> m_additions = new Vector<>();
+ private NetworkTableEntry m_namesEntry;
+ private NetworkTableEntry m_idsEntry;
+ private NetworkTableEntry m_cancelEntry;
+ /**
+ * A list of all {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It is
+ * created lazily.
+ */
+ @SuppressWarnings("PMD.LooseCoupling")
+ private Vector<ButtonScheduler> m_buttons;
+ private boolean m_runningCommandsChanged;
+
+ /**
+ * Instantiates a {@link Scheduler}.
+ */
+ private Scheduler() {
+ HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+ setName("Scheduler");
+ }
+
+ /**
+ * Adds the command to the {@link Scheduler}. This will not add the {@link Command} immediately,
+ * but will instead wait for the proper time in the {@link Scheduler#run()} loop before doing so.
+ * The command returns immediately and does nothing if given null.
+ *
+ * <p> Adding a {@link Command} to the {@link Scheduler} involves the {@link Scheduler} removing
+ * any {@link Command} which has shared requirements. </p>
+ *
+ * @param command the command to add
+ */
+ public void add(Command command) {
+ if (command != null) {
+ m_additions.addElement(command);
+ }
+ }
+
+ /**
+ * Adds a button to the {@link Scheduler}. The {@link Scheduler} will poll the button during its
+ * {@link Scheduler#run()}.
+ *
+ * @param button the button to add
+ */
+ @SuppressWarnings("PMD.UseArrayListInsteadOfVector")
+ public void addButton(ButtonScheduler button) {
+ if (m_buttons == null) {
+ m_buttons = new Vector<>();
+ }
+ m_buttons.addElement(button);
+ }
+
+ /**
+ * Adds a command immediately to the {@link Scheduler}. This should only be called in the {@link
+ * Scheduler#run()} loop. Any command with conflicting requirements will be removed, unless it is
+ * uninterruptable. Giving <code>null</code> does nothing.
+ *
+ * @param command the {@link Command} to add
+ */
+ @SuppressWarnings({"MethodName", "PMD.CyclomaticComplexity"})
+ private void _add(Command command) {
+ if (command == null) {
+ return;
+ }
+
+ // Check to make sure no adding during adding
+ if (m_adding) {
+ System.err.println("WARNING: Can not start command from cancel method. Ignoring:" + command);
+ return;
+ }
+
+ // Only add if not already in
+ if (!m_commandTable.containsKey(command)) {
+ // Check that the requirements can be had
+ Enumeration requirements = command.getRequirements();
+ while (requirements.hasMoreElements()) {
+ Subsystem lock = (Subsystem) requirements.nextElement();
+ if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
+ return;
+ }
+ }
+
+ // Give it the requirements
+ m_adding = true;
+ requirements = command.getRequirements();
+ while (requirements.hasMoreElements()) {
+ Subsystem lock = (Subsystem) requirements.nextElement();
+ if (lock.getCurrentCommand() != null) {
+ lock.getCurrentCommand().cancel();
+ remove(lock.getCurrentCommand());
+ }
+ lock.setCurrentCommand(command);
+ }
+ m_adding = false;
+
+ // Add it to the list
+ LinkedListElement element = new LinkedListElement();
+ element.setData(command);
+ if (m_firstCommand == null) {
+ m_firstCommand = m_lastCommand = element;
+ } else {
+ m_lastCommand.add(element);
+ m_lastCommand = element;
+ }
+ m_commandTable.put(command, element);
+
+ m_runningCommandsChanged = true;
+
+ command.startRunning();
+ }
+ }
+
+ /**
+ * Runs a single iteration of the loop. This method should be called often in order to have a
+ * functioning {@link Command} system. The loop has five stages:
+ *
+ * <ol> <li>Poll the Buttons</li> <li>Execute/Remove the Commands</li> <li>Send values to
+ * SmartDashboard</li> <li>Add Commands</li> <li>Add Defaults</li> </ol>
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ public void run() {
+ m_runningCommandsChanged = false;
+
+ if (m_disabled) {
+ return;
+ } // Don't run when m_disabled
+
+ // Get button input (going backwards preserves button priority)
+ if (m_buttons != null) {
+ for (int i = m_buttons.size() - 1; i >= 0; i--) {
+ m_buttons.elementAt(i).execute();
+ }
+ }
+
+ // Call every subsystem's periodic method
+ Enumeration subsystems = m_subsystems.getElements();
+ while (subsystems.hasMoreElements()) {
+ ((Subsystem) subsystems.nextElement()).periodic();
+ }
+
+ // Loop through the commands
+ LinkedListElement element = m_firstCommand;
+ while (element != null) {
+ Command command = element.getData();
+ element = element.getNext();
+ if (!command.run()) {
+ remove(command);
+ m_runningCommandsChanged = true;
+ }
+ }
+
+ // Add the new things
+ for (int i = 0; i < m_additions.size(); i++) {
+ _add(m_additions.elementAt(i));
+ }
+ m_additions.removeAllElements();
+
+ // Add in the defaults
+ Enumeration locks = m_subsystems.getElements();
+ while (locks.hasMoreElements()) {
+ Subsystem lock = (Subsystem) locks.nextElement();
+ if (lock.getCurrentCommand() == null) {
+ _add(lock.getDefaultCommand());
+ }
+ lock.confirmCommand();
+ }
+ }
+
+ /**
+ * Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link Scheduler} might
+ * know if a default {@link Command} needs to be run. All {@link Subsystem Subsystems} should call
+ * this.
+ *
+ * @param system the system
+ */
+ void registerSubsystem(Subsystem system) {
+ if (system != null) {
+ m_subsystems.add(system);
+ }
+ }
+
+ /**
+ * Removes the {@link Command} from the {@link Scheduler}.
+ *
+ * @param command the command to remove
+ */
+ void remove(Command command) {
+ if (command == null || !m_commandTable.containsKey(command)) {
+ return;
+ }
+ LinkedListElement element = m_commandTable.get(command);
+ m_commandTable.remove(command);
+
+ if (element.equals(m_lastCommand)) {
+ m_lastCommand = element.getPrevious();
+ }
+ if (element.equals(m_firstCommand)) {
+ m_firstCommand = element.getNext();
+ }
+ element.remove();
+
+ Enumeration requirements = command.getRequirements();
+ while (requirements.hasMoreElements()) {
+ ((Subsystem) requirements.nextElement()).setCurrentCommand(null);
+ }
+
+ command.removed();
+ }
+
+ /**
+ * Removes all commands.
+ */
+ public void removeAll() {
+ // TODO: Confirm that this works with "uninteruptible" commands
+ while (m_firstCommand != null) {
+ remove(m_firstCommand.getData());
+ }
+ }
+
+ /**
+ * Disable the command scheduler.
+ */
+ public void disable() {
+ m_disabled = true;
+ }
+
+ /**
+ * Enable the command scheduler.
+ */
+ public void enable() {
+ m_disabled = false;
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Scheduler");
+ m_namesEntry = builder.getEntry("Names");
+ m_idsEntry = builder.getEntry("Ids");
+ m_cancelEntry = builder.getEntry("Cancel");
+ builder.setUpdateTable(() -> {
+ if (m_namesEntry != null && m_idsEntry != null && m_cancelEntry != null) {
+ // Get the commands to cancel
+ double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+ if (toCancel.length > 0) {
+ for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
+ for (double d : toCancel) {
+ if (e.getData().hashCode() == d) {
+ e.getData().cancel();
+ }
+ }
+ }
+ m_cancelEntry.setDoubleArray(new double[0]);
+ }
+
+ if (m_runningCommandsChanged) {
+ // Set the the running commands
+ int number = 0;
+ for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
+ number++;
+ }
+ String[] commands = new String[number];
+ double[] ids = new double[number];
+ number = 0;
+ for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
+ commands[number] = e.getData().getName();
+ ids[number] = e.getData().hashCode();
+ number++;
+ }
+ m_namesEntry.setStringArray(commands);
+ m_idsEntry.setDoubleArray(ids);
+ }
+ }
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java
new file mode 100644
index 0000000..4b05467
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+import java.util.Vector;
+
+@SuppressWarnings("all")
+/**
+ * A set.
+ */
+class Set {
+ private Vector m_set = new Vector();
+
+ public Set() {
+ }
+
+ public void add(Object o) {
+ if (m_set.contains(o)) {
+ return;
+ }
+ m_set.addElement(o);
+ }
+
+ public void add(Set s) {
+ Enumeration stuff = s.getElements();
+ for (Enumeration e = stuff; e.hasMoreElements(); ) {
+ add(e.nextElement());
+ }
+ }
+
+ public void clear() {
+ m_set.clear();
+ }
+
+ public boolean contains(Object o) {
+ return m_set.contains(o);
+ }
+
+ public Enumeration getElements() {
+ return m_set.elements();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
new file mode 100644
index 0000000..6dd7db8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link StartCommand} will call the {@link Command#start() start()} method of another command
+ * when it is initialized and will finish immediately.
+ */
+public class StartCommand extends InstantCommand {
+ /**
+ * The command to fork.
+ */
+ private final Command m_commandToFork;
+
+ /**
+ * Instantiates a {@link StartCommand} which will start the given command whenever its {@link
+ * Command#initialize() initialize()} is called.
+ *
+ * @param commandToStart the {@link Command} to start
+ */
+ public StartCommand(Command commandToStart) {
+ super("Start(" + commandToStart + ")");
+ m_commandToFork = commandToStart;
+ }
+
+ @Override
+ protected void initialize() {
+ m_commandToFork.start();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
new file mode 100644
index 0000000..0da80c2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
@@ -0,0 +1,210 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Collections;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * This class defines a major component of the robot.
+ *
+ * <p> A good example of a subsystem is the driveline, or a claw if the robot has one. </p>
+ *
+ * <p> All motors should be a part of a subsystem. For instance, all the wheel motors should be a
+ * part of some kind of "Driveline" subsystem. </p>
+ *
+ * <p> Subsystems are used within the command system as requirements for {@link Command}. Only one
+ * command which requires a subsystem can run at a time. Also, subsystems can have default commands
+ * which are started if there is no command running which requires this subsystem. </p>
+ *
+ * @see Command
+ */
+public abstract class Subsystem extends SendableBase {
+ /**
+ * Whether or not getDefaultCommand() was called.
+ */
+ private boolean m_initializedDefaultCommand;
+ /**
+ * The current command.
+ */
+ private Command m_currentCommand;
+ private boolean m_currentCommandChanged;
+
+ /**
+ * The default command.
+ */
+ private Command m_defaultCommand;
+
+ /**
+ * Creates a subsystem with the given name.
+ *
+ * @param name the name of the subsystem
+ */
+ public Subsystem(String name) {
+ setName(name, name);
+ Scheduler.getInstance().registerSubsystem(this);
+ }
+
+ /**
+ * Creates a subsystem. This will set the name to the name of the class.
+ */
+ public Subsystem() {
+ String name = getClass().getName();
+ name = name.substring(name.lastIndexOf('.') + 1);
+ setName(name, name);
+ Scheduler.getInstance().registerSubsystem(this);
+ m_currentCommandChanged = true;
+ }
+
+ /**
+ * Initialize the default command for a subsystem By default subsystems have no default command,
+ * but if they do, the default command is set with this method. It is called on all Subsystems by
+ * CommandBase in the users program after all the Subsystems are created.
+ */
+ protected abstract void initDefaultCommand();
+
+ /**
+ * When the run method of the scheduler is called this method will be called.
+ */
+ public void periodic() {
+ // Override me!
+ }
+
+ /**
+ * Sets the default command. If this is not called or is called with null, then there will be no
+ * default command for the subsystem.
+ *
+ * <p> <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
+ * singleton. </p>
+ *
+ * @param command the default command (or null if there should be none)
+ * @throws IllegalUseOfCommandException if the command does not require the subsystem
+ */
+ public void setDefaultCommand(Command command) {
+ if (command == null) {
+ m_defaultCommand = null;
+ } else {
+ if (!Collections.list(command.getRequirements()).contains(this)) {
+ throw new IllegalUseOfCommandException("A default command must require the subsystem");
+ }
+ m_defaultCommand = command;
+ }
+ }
+
+ /**
+ * Returns the default command (or null if there is none).
+ *
+ * @return the default command
+ */
+ public Command getDefaultCommand() {
+ if (!m_initializedDefaultCommand) {
+ m_initializedDefaultCommand = true;
+ initDefaultCommand();
+ }
+ return m_defaultCommand;
+ }
+
+ /**
+ * Returns the default command name, or empty string is there is none.
+ *
+ * @return the default command name
+ */
+ public String getDefaultCommandName() {
+ Command defaultCommand = getDefaultCommand();
+ if (defaultCommand != null) {
+ return defaultCommand.getName();
+ } else {
+ return "";
+ }
+ }
+
+ /**
+ * Sets the current command.
+ *
+ * @param command the new current command
+ */
+ void setCurrentCommand(Command command) {
+ m_currentCommand = command;
+ m_currentCommandChanged = true;
+ }
+
+ /**
+ * Call this to alert Subsystem that the current command is actually the command. Sometimes, the
+ * {@link Subsystem} is told that it has no command while the {@link Scheduler} is going through
+ * the loop, only to be soon after given a new one. This will avoid that situation.
+ */
+ void confirmCommand() {
+ if (m_currentCommandChanged) {
+ m_currentCommandChanged = false;
+ }
+ }
+
+ /**
+ * Returns the command which currently claims this subsystem.
+ *
+ * @return the command which currently claims this subsystem
+ */
+ public Command getCurrentCommand() {
+ return m_currentCommand;
+ }
+
+ /**
+ * Returns the current command name, or empty string if no current command.
+ *
+ * @return the current command name
+ */
+ public String getCurrentCommandName() {
+ Command currentCommand = getCurrentCommand();
+ if (currentCommand != null) {
+ return currentCommand.getName();
+ } else {
+ return "";
+ }
+ }
+
+ /**
+ * Associate a {@link Sendable} with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ public void addChild(String name, Sendable child) {
+ child.setName(getSubsystem(), name);
+ LiveWindow.add(child);
+ }
+
+ /**
+ * Associate a {@link Sendable} with this Subsystem.
+ *
+ * @param child sendable
+ */
+ public void addChild(Sendable child) {
+ child.setSubsystem(getSubsystem());
+ LiveWindow.add(child);
+ }
+
+ @Override
+ public String toString() {
+ return getSubsystem();
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Subsystem");
+
+ builder.addBooleanProperty(".hasDefault", () -> m_defaultCommand != null, null);
+ builder.addStringProperty(".default", this::getDefaultCommandName, null);
+ builder.addBooleanProperty(".hasCommand", () -> m_currentCommand != null, null);
+ builder.addStringProperty(".command", this::getCurrentCommandName, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
new file mode 100644
index 0000000..6c9193b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link TimedCommand} will wait for a timeout before finishing.
+ * {@link TimedCommand} is used to execute a command for a given amount of time.
+ */
+public class TimedCommand extends Command {
+ /**
+ * Instantiates a TimedCommand with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time the command takes to run (seconds)
+ */
+ public TimedCommand(String name, double timeout) {
+ super(name, timeout);
+ }
+
+ /**
+ * Instantiates a TimedCommand with the given timeout.
+ *
+ * @param timeout the time the command takes to run (seconds)
+ */
+ public TimedCommand(double timeout) {
+ super(timeout);
+ }
+
+ /**
+ * Instantiates a TimedCommand with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time the command takes to run (seconds)
+ * @param subsystem the subsystem that this command requires
+ */
+ public TimedCommand(String name, double timeout, Subsystem subsystem) {
+ super(name, timeout, subsystem);
+ }
+
+ /**
+ * Instantiates a TimedCommand with the given timeout.
+ *
+ * @param timeout the time the command takes to run (seconds)
+ * @param subsystem the subsystem that this command requires
+ */
+ public TimedCommand(double timeout, Subsystem subsystem) {
+ super(timeout, subsystem);
+ }
+
+ /**
+ * Ends command when timed out.
+ */
+ @Override
+ protected boolean isFinished() {
+ return isTimedOut();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
new file mode 100644
index 0000000..1af7c1f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link WaitCommand} will wait for a certain amount of time before finishing. It is useful if
+ * you want a {@link CommandGroup} to pause for a moment.
+ *
+ * @see CommandGroup
+ */
+public class WaitCommand extends TimedCommand {
+ /**
+ * Instantiates a {@link WaitCommand} with the given timeout.
+ *
+ * @param timeout the time the command takes to run (seconds)
+ */
+ public WaitCommand(double timeout) {
+ this("Wait(" + timeout + ")", timeout);
+ }
+
+ /**
+ * Instantiates a {@link WaitCommand} with the given timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time the command takes to run (seconds)
+ */
+ public WaitCommand(String name, double timeout) {
+ super(name, timeout);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
new file mode 100644
index 0000000..27795c0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * This command will only finish if whatever {@link CommandGroup} it is in has no active children.
+ * If it is not a part of a {@link CommandGroup}, then it will finish immediately. If it is itself
+ * an active child, then the {@link CommandGroup} will never end.
+ *
+ * <p>This class is useful for the situation where you want to allow anything running in parallel
+ * to finish, before continuing in the main {@link CommandGroup} sequence.
+ */
+public class WaitForChildren extends Command {
+ @Override
+ protected boolean isFinished() {
+ return getGroup() == null || getGroup().m_children.isEmpty();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
new file mode 100644
index 0000000..799fa8c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * WaitUntilCommand - waits until an absolute game time. This will wait until the game clock reaches
+ * some value, then continue to the next command.
+ */
+public class WaitUntilCommand extends Command {
+ private final double m_time;
+
+ public WaitUntilCommand(double time) {
+ super("WaitUntil(" + time + ")");
+ m_time = time;
+ }
+
+ /**
+ * Check if we've reached the actual finish time.
+ */
+ @Override
+ public boolean isFinished() {
+ return Timer.getMatchTime() >= m_time;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
new file mode 100644
index 0000000..9355adc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -0,0 +1,427 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import java.util.StringJoiner;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
+ * base, "tank drive", or West Coast Drive.
+ *
+ * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
+ * (e.g., 6WD or 8WD). This class takes a SpeedController per side. For four and
+ * six motor drivetrains, construct and pass in {@link edu.wpi.first.wpilibj.SpeedControllerGroup}
+ * instances as follows.
+ *
+ * <p>Four motor drivetrain:
+ * <pre><code>
+ * public class Robot {
+ * Spark m_frontLeft = new Spark(1);
+ * Spark m_rearLeft = new Spark(2);
+ * SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
+ *
+ * Spark m_frontRight = new Spark(3);
+ * Spark m_rearRight = new Spark(4);
+ * SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
+ *
+ * DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ * </code></pre>
+ *
+ * <p>Six motor drivetrain:
+ * <pre><code>
+ * public class Robot {
+ * Spark m_frontLeft = new Spark(1);
+ * Spark m_midLeft = new Spark(2);
+ * Spark m_rearLeft = new Spark(3);
+ * SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
+ *
+ * Spark m_frontRight = new Spark(4);
+ * Spark m_midRight = new Spark(5);
+ * Spark m_rearRight = new Spark(6);
+ * SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
+ *
+ * DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ * </code></pre>
+ *
+ * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
+ *
+ * <p>Drive base diagram:
+ * <pre>
+ * |_______|
+ * | | | |
+ * | |
+ * |_|___|_|
+ * | |
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a differential drive
+ * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is
+ * usually unnecessary.
+ *
+ * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
+ * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
+ *
+ * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
+ * positive.
+ *
+ * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
+ * be set to 0, and larger values will be scaled so that the full range is still used. This
+ * deadband value can be changed with {@link #setDeadband}.
+ *
+ * <p>RobotDrive porting guide:
+ * <br>{@link #tankDrive(double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used.
+ * <br>{@link #arcadeDrive(double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used
+ * and the the rotation input is inverted eg arcadeDrive(y, -rotation)
+ * <br>{@link #curvatureDrive(double, double, boolean)} is similar in concept to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn
+ * mode. However, it is not designed to give exactly the same response.
+ */
+public class DifferentialDrive extends RobotDriveBase {
+ public static final double kDefaultQuickStopThreshold = 0.2;
+ public static final double kDefaultQuickStopAlpha = 0.1;
+
+ private static int instances;
+
+ private final SpeedController m_leftMotor;
+ private final SpeedController m_rightMotor;
+
+ private double m_quickStopThreshold = kDefaultQuickStopThreshold;
+ private double m_quickStopAlpha = kDefaultQuickStopAlpha;
+ private double m_quickStopAccumulator;
+ private double m_rightSideInvertMultiplier = -1.0;
+ private boolean m_reported;
+
+ /**
+ * Construct a DifferentialDrive.
+ *
+ * <p>To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be
+ * inverted, do so before passing it in.
+ */
+ public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) {
+ verify(leftMotor, rightMotor);
+ m_leftMotor = leftMotor;
+ m_rightMotor = rightMotor;
+ addChild(m_leftMotor);
+ addChild(m_rightMotor);
+ instances++;
+ setName("DifferentialDrive", instances);
+ }
+
+ /**
+ * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
+ * The exception's error message will specify all null motors, e.g. {@code
+ * NullPointerException("leftMotor, rightMotor")}, to give as much information as possible to
+ * the programmer.
+ *
+ * @throws NullPointerException if any of the given motors are null
+ */
+ @SuppressWarnings("PMD.AvoidThrowingNullPointerException")
+ private void verify(SpeedController leftMotor, SpeedController rightMotor) {
+ if (leftMotor != null && rightMotor != null) {
+ return;
+ }
+ StringJoiner joiner = new StringJoiner(", ");
+ if (leftMotor == null) {
+ joiner.add("leftMotor");
+ }
+ if (rightMotor == null) {
+ joiner.add("rightMotor");
+ }
+ throw new NullPointerException(joiner.toString());
+ }
+
+ /**
+ * Arcade drive method for differential drive platform.
+ * The calculated values will be squared to decrease sensitivity at low speeds.
+ *
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void arcadeDrive(double xSpeed, double zRotation) {
+ arcadeDrive(xSpeed, zRotation, true);
+ }
+
+ /**
+ * Arcade drive method for differential drive platform.
+ *
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param squareInputs If set, decreases the input sensitivity at low speeds.
+ */
+ @SuppressWarnings("ParameterName")
+ public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 2,
+ tInstances.kRobotDrive2_DifferentialArcade);
+ m_reported = true;
+ }
+
+ xSpeed = limit(xSpeed);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ zRotation = limit(zRotation);
+ zRotation = applyDeadband(zRotation, m_deadband);
+
+ // Square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power.
+ if (squareInputs) {
+ xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
+ zRotation = Math.copySign(zRotation * zRotation, zRotation);
+ }
+
+ double leftMotorOutput;
+ double rightMotorOutput;
+
+ double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
+
+ if (xSpeed >= 0.0) {
+ // First quadrant, else second quadrant
+ if (zRotation >= 0.0) {
+ leftMotorOutput = maxInput;
+ rightMotorOutput = xSpeed - zRotation;
+ } else {
+ leftMotorOutput = xSpeed + zRotation;
+ rightMotorOutput = maxInput;
+ }
+ } else {
+ // Third quadrant, else fourth quadrant
+ if (zRotation >= 0.0) {
+ leftMotorOutput = xSpeed + zRotation;
+ rightMotorOutput = maxInput;
+ } else {
+ leftMotorOutput = maxInput;
+ rightMotorOutput = xSpeed - zRotation;
+ }
+ }
+
+ m_leftMotor.set(limit(leftMotorOutput) * m_maxOutput);
+ m_rightMotor.set(limit(rightMotorOutput) * m_maxOutput * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Curvature drive method for differential drive platform.
+ *
+ * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
+ * heading change. This makes the robot more controllable at high speeds. Also handles the
+ * robot's quick turn functionality - "quick turn" overrides constant-curvature turning for
+ * turn-in-place maneuvers.
+ *
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param isQuickTurn If set, overrides constant-curvature turning for
+ * turn-in-place maneuvers.
+ */
+ @SuppressWarnings({"ParameterName", "PMD.CyclomaticComplexity"})
+ public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 2,
+ tInstances.kRobotDrive2_DifferentialCurvature);
+ m_reported = true;
+ }
+
+ xSpeed = limit(xSpeed);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ zRotation = limit(zRotation);
+ zRotation = applyDeadband(zRotation, m_deadband);
+
+ double angularPower;
+ boolean overPower;
+
+ if (isQuickTurn) {
+ if (Math.abs(xSpeed) < m_quickStopThreshold) {
+ m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator
+ + m_quickStopAlpha * limit(zRotation) * 2;
+ }
+ overPower = true;
+ angularPower = zRotation;
+ } else {
+ overPower = false;
+ angularPower = Math.abs(xSpeed) * zRotation - m_quickStopAccumulator;
+
+ if (m_quickStopAccumulator > 1) {
+ m_quickStopAccumulator -= 1;
+ } else if (m_quickStopAccumulator < -1) {
+ m_quickStopAccumulator += 1;
+ } else {
+ m_quickStopAccumulator = 0.0;
+ }
+ }
+
+ double leftMotorOutput = xSpeed + angularPower;
+ double rightMotorOutput = xSpeed - angularPower;
+
+ // If rotation is overpowered, reduce both outputs to within acceptable range
+ if (overPower) {
+ if (leftMotorOutput > 1.0) {
+ rightMotorOutput -= leftMotorOutput - 1.0;
+ leftMotorOutput = 1.0;
+ } else if (rightMotorOutput > 1.0) {
+ leftMotorOutput -= rightMotorOutput - 1.0;
+ rightMotorOutput = 1.0;
+ } else if (leftMotorOutput < -1.0) {
+ rightMotorOutput -= leftMotorOutput + 1.0;
+ leftMotorOutput = -1.0;
+ } else if (rightMotorOutput < -1.0) {
+ leftMotorOutput -= rightMotorOutput + 1.0;
+ rightMotorOutput = -1.0;
+ }
+ }
+
+ // Normalize the wheel speeds
+ double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
+ if (maxMagnitude > 1.0) {
+ leftMotorOutput /= maxMagnitude;
+ rightMotorOutput /= maxMagnitude;
+ }
+
+ m_leftMotor.set(leftMotorOutput * m_maxOutput);
+ m_rightMotor.set(rightMotorOutput * m_maxOutput * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Tank drive method for differential drive platform.
+ * The calculated values will be squared to decrease sensitivity at low speeds.
+ *
+ * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ */
+ public void tankDrive(double leftSpeed, double rightSpeed) {
+ tankDrive(leftSpeed, rightSpeed, true);
+ }
+
+ /**
+ * Tank drive method for differential drive platform.
+ *
+ * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+ * positive.
+ * @param squareInputs If set, decreases the input sensitivity at low speeds.
+ */
+ public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 2,
+ tInstances.kRobotDrive2_DifferentialTank);
+ m_reported = true;
+ }
+
+ leftSpeed = limit(leftSpeed);
+ leftSpeed = applyDeadband(leftSpeed, m_deadband);
+
+ rightSpeed = limit(rightSpeed);
+ rightSpeed = applyDeadband(rightSpeed, m_deadband);
+
+ // Square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power.
+ if (squareInputs) {
+ leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
+ rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
+ }
+
+ m_leftMotor.set(leftSpeed * m_maxOutput);
+ m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Sets the QuickStop speed threshold in curvature drive.
+ *
+ * <p>QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn.
+ *
+ * <p>While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value
+ * outputted by the low-pass filter when the robot's speed along the X axis is below the
+ * threshold. When QuickTurn is disabled, the accumulator's value is applied against the computed
+ * angular power request to slow the robot's rotation.
+ *
+ * @param threshold X speed below which quick stop accumulator will receive rotation rate values
+ * [0..1.0].
+ */
+ public void setQuickStopThreshold(double threshold) {
+ m_quickStopThreshold = threshold;
+ }
+
+ /**
+ * Sets the low-pass filter gain for QuickStop in curvature drive.
+ *
+ * <p>The low-pass filter filters incoming rotation rate commands to smooth out high frequency
+ * changes.
+ *
+ * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes.
+ * Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and
+ * above 2.0 are unstable.
+ */
+ public void setQuickStopAlpha(double alpha) {
+ m_quickStopAlpha = alpha;
+ }
+
+ /**
+ * Gets if the power sent to the right side of the drivetrain is multipled by -1.
+ *
+ * @return true if the right side is inverted
+ */
+ public boolean isRightSideInverted() {
+ return m_rightSideInvertMultiplier == -1.0;
+ }
+
+ /**
+ * Sets if the power sent to the right side of the drivetrain should be multipled by -1.
+ *
+ * @param rightSideInverted true if right side power should be multipled by -1
+ */
+ public void setRightSideInverted(boolean rightSideInverted) {
+ m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+ }
+
+ @Override
+ public void stopMotor() {
+ m_leftMotor.stopMotor();
+ m_rightMotor.stopMotor();
+ feed();
+ }
+
+ @Override
+ public String getDescription() {
+ return "DifferentialDrive";
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("DifferentialDrive");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
+ builder.addDoubleProperty(
+ "Right Motor Speed",
+ () -> m_rightMotor.get() * m_rightSideInvertMultiplier,
+ x -> m_rightMotor.set(x * m_rightSideInvertMultiplier));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
new file mode 100644
index 0000000..165ba70
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import java.util.StringJoiner;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A class for driving Killough drive platforms.
+ *
+ * <p>Killough drives are triangular with one omni wheel on each corner.
+ *
+ * <p>Drive base diagram:
+ * <pre>
+ * /_____\
+ * / \ / \
+ * \ /
+ * ---
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a Killough drive. The
+ * default wheel vectors are parallel to their respective opposite sides, but can be overridden. See
+ * the constructor for more information.
+ *
+ * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
+ * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
+ *
+ * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
+ * positive.
+ */
+public class KilloughDrive extends RobotDriveBase {
+ public static final double kDefaultLeftMotorAngle = 60.0;
+ public static final double kDefaultRightMotorAngle = 120.0;
+ public static final double kDefaultBackMotorAngle = 270.0;
+
+ private static int instances;
+
+ private SpeedController m_leftMotor;
+ private SpeedController m_rightMotor;
+ private SpeedController m_backMotor;
+
+ private Vector2d m_leftVec;
+ private Vector2d m_rightVec;
+ private Vector2d m_backVec;
+
+ private boolean m_reported;
+
+ /**
+ * Construct a Killough drive with the given motors and default motor angles.
+ *
+ * <p>The default motor angles make the wheels on each corner parallel to their respective
+ * opposite sides.
+ *
+ * <p>If a motor needs to be inverted, do so before passing it in.
+ *
+ * @param leftMotor The motor on the left corner.
+ * @param rightMotor The motor on the right corner.
+ * @param backMotor The motor on the back corner.
+ */
+ public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
+ SpeedController backMotor) {
+ this(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle, kDefaultRightMotorAngle,
+ kDefaultBackMotorAngle);
+ }
+
+ /**
+ * Construct a Killough drive with the given motors.
+ *
+ * <p>Angles are measured in degrees clockwise from the positive X axis.
+ *
+ * @param leftMotor The motor on the left corner.
+ * @param rightMotor The motor on the right corner.
+ * @param backMotor The motor on the back corner.
+ * @param leftMotorAngle The angle of the left wheel's forward direction of travel.
+ * @param rightMotorAngle The angle of the right wheel's forward direction of travel.
+ * @param backMotorAngle The angle of the back wheel's forward direction of travel.
+ */
+ public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
+ SpeedController backMotor, double leftMotorAngle, double rightMotorAngle,
+ double backMotorAngle) {
+ verify(leftMotor, rightMotor, backMotor);
+ m_leftMotor = leftMotor;
+ m_rightMotor = rightMotor;
+ m_backMotor = backMotor;
+ m_leftVec = new Vector2d(Math.cos(leftMotorAngle * (Math.PI / 180.0)),
+ Math.sin(leftMotorAngle * (Math.PI / 180.0)));
+ m_rightVec = new Vector2d(Math.cos(rightMotorAngle * (Math.PI / 180.0)),
+ Math.sin(rightMotorAngle * (Math.PI / 180.0)));
+ m_backVec = new Vector2d(Math.cos(backMotorAngle * (Math.PI / 180.0)),
+ Math.sin(backMotorAngle * (Math.PI / 180.0)));
+ addChild(m_leftMotor);
+ addChild(m_rightMotor);
+ addChild(m_backMotor);
+ instances++;
+ setName("KilloughDrive", instances);
+ }
+
+ /**
+ * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
+ * The exception's error message will specify all null motors, e.g. {@code
+ * NullPointerException("leftMotor, rightMotor")}, to give as much information as possible to
+ * the programmer.
+ *
+ * @throws NullPointerException if any of the given motors are null
+ */
+ @SuppressWarnings("PMD.AvoidThrowingNullPointerException")
+ private void verify(SpeedController leftMotor, SpeedController rightMotor,
+ SpeedController backMotor) {
+ if (leftMotor != null && rightMotor != null && backMotor != null) {
+ return;
+ }
+ StringJoiner joiner = new StringJoiner(", ");
+ if (leftMotor == null) {
+ joiner.add("leftMotor");
+ }
+ if (rightMotor == null) {
+ joiner.add("rightMotor");
+ }
+ if (backMotor == null) {
+ joiner.add("backMotor");
+ }
+ throw new NullPointerException(joiner.toString());
+ }
+
+ /**
+ * Drive method for Killough platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
+ driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
+ }
+
+ /**
+ * Drive method for Killough platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use
+ * this to implement field-oriented controls.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation,
+ double gyroAngle) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 3,
+ tInstances.kRobotDrive2_KilloughCartesian);
+ m_reported = true;
+ }
+
+ ySpeed = limit(ySpeed);
+ ySpeed = applyDeadband(ySpeed, m_deadband);
+
+ xSpeed = limit(xSpeed);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ // Compensate for gyro angle.
+ Vector2d input = new Vector2d(ySpeed, xSpeed);
+ input.rotate(-gyroAngle);
+
+ double[] wheelSpeeds = new double[3];
+ wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation;
+ wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation;
+ wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation;
+
+ normalize(wheelSpeeds);
+
+ m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput);
+ m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput);
+ m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * m_maxOutput);
+
+ feed();
+ }
+
+ /**
+ * Drive method for Killough platform.
+ *
+ * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
+ * drives (translation) is independent from its angle or rotation rate.
+ *
+ * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
+ * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drivePolar(double magnitude, double angle, double zRotation) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 3,
+ tInstances.kRobotDrive2_KilloughPolar);
+ m_reported = true;
+ }
+
+ driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
+ magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
+ }
+
+ @Override
+ public void stopMotor() {
+ m_leftMotor.stopMotor();
+ m_rightMotor.stopMotor();
+ m_backMotor.stopMotor();
+ feed();
+ }
+
+ @Override
+ public String getDescription() {
+ return "KilloughDrive";
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("KilloughDrive");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
+ builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set);
+ builder.addDoubleProperty("Back Motor Speed", m_backMotor::get, m_backMotor::set);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
new file mode 100644
index 0000000..acdc4d7
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -0,0 +1,259 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import java.util.StringJoiner;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A class for driving Mecanum drive platforms.
+ *
+ * <p>Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in
+ * 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles
+ * should form an X across the robot. Each drive() function provides different inverse kinematic
+ * relations for a Mecanum drive robot.
+ *
+ * <p>Drive base diagram:
+ * <pre>
+ * \\_______/
+ * \\ | | /
+ * | |
+ * /_|___|_\\
+ * / \\
+ * </pre>
+ *
+ * <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
+ * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is
+ * usually unnecessary.
+ *
+ * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
+ * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
+ *
+ * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
+ * positive.
+ *
+ * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
+ * be set to 0, and larger values will be scaled so that the full range is still used. This
+ * deadband value can be changed with {@link #setDeadband}.
+ *
+ * <p>RobotDrive porting guide:
+ * <br>In MecanumDrive, the right side speed controllers are automatically inverted, while in
+ * RobotDrive, no speed controllers are automatically inverted.
+ * <br>{@link #driveCartesian(double, double, double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Cartesian(double, double, double, double)}
+ * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted compared to
+ * RobotDrive (eg driveCartesian(xSpeed, -ySpeed, zRotation, -gyroAngle).
+ * <br>{@link #drivePolar(double, double, double)} is equivalent to
+ * {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Polar(double, double, double)} if a
+ * deadband of 0 is used.
+ */
+public class MecanumDrive extends RobotDriveBase {
+ private static int instances;
+
+ private final SpeedController m_frontLeftMotor;
+ private final SpeedController m_rearLeftMotor;
+ private final SpeedController m_frontRightMotor;
+ private final SpeedController m_rearRightMotor;
+
+ private double m_rightSideInvertMultiplier = -1.0;
+ private boolean m_reported;
+
+ /**
+ * Construct a MecanumDrive.
+ *
+ * <p>If a motor needs to be inverted, do so before passing it in.
+ */
+ public MecanumDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
+ SpeedController frontRightMotor, SpeedController rearRightMotor) {
+ verify(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
+ m_frontLeftMotor = frontLeftMotor;
+ m_rearLeftMotor = rearLeftMotor;
+ m_frontRightMotor = frontRightMotor;
+ m_rearRightMotor = rearRightMotor;
+ addChild(m_frontLeftMotor);
+ addChild(m_rearLeftMotor);
+ addChild(m_frontRightMotor);
+ addChild(m_rearRightMotor);
+ instances++;
+ setName("MecanumDrive", instances);
+ }
+
+ /**
+ * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
+ * The exception's error message will specify all null motors, e.g. {@code
+ * NullPointerException("frontLeftMotor, rearRightMotor")}, to give as much information as
+ * possible to the programmer.
+ *
+ * @throws NullPointerException if any of the given motors are null
+ */
+ @SuppressWarnings({"PMD.AvoidThrowingNullPointerException", "PMD.CyclomaticComplexity"})
+ private void verify(SpeedController frontLeft, SpeedController rearLeft,
+ SpeedController frontRight, SpeedController rearRightMotor) {
+ if (frontLeft != null && rearLeft != null && frontRight != null && rearRightMotor != null) {
+ return;
+ }
+ StringJoiner joiner = new StringJoiner(", ");
+ if (frontLeft == null) {
+ joiner.add("frontLeftMotor");
+ }
+ if (rearLeft == null) {
+ joiner.add("rearLeftMotor");
+ }
+ if (frontRight == null) {
+ joiner.add("frontRightMotor");
+ }
+ if (rearRightMotor == null) {
+ joiner.add("rearRightMotor");
+ }
+ throw new NullPointerException(joiner.toString());
+ }
+
+ /**
+ * Drive method for Mecanum platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
+ driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
+ }
+
+ /**
+ * Drive method for Mecanum platform.
+ *
+ * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+ * from its angle or rotation rate.
+ *
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use
+ * this to implement field-oriented controls.
+ */
+ @SuppressWarnings("ParameterName")
+ public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 4,
+ tInstances.kRobotDrive2_MecanumCartesian);
+ m_reported = true;
+ }
+
+ ySpeed = limit(ySpeed);
+ ySpeed = applyDeadband(ySpeed, m_deadband);
+
+ xSpeed = limit(xSpeed);
+ xSpeed = applyDeadband(xSpeed, m_deadband);
+
+ // Compensate for gyro angle.
+ Vector2d input = new Vector2d(ySpeed, xSpeed);
+ input.rotate(-gyroAngle);
+
+ double[] wheelSpeeds = new double[4];
+ wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
+ wheelSpeeds[MotorType.kFrontRight.value] = -input.x + input.y - zRotation;
+ wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
+ wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
+
+ normalize(wheelSpeeds);
+
+ m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
+ m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput
+ * m_rightSideInvertMultiplier);
+ m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
+ m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput
+ * m_rightSideInvertMultiplier);
+
+ feed();
+ }
+
+ /**
+ * Drive method for Mecanum platform.
+ *
+ * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
+ * drives (translation) is independent from its angle or rotation rate.
+ *
+ * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
+ * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
+ * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+ * positive.
+ */
+ @SuppressWarnings("ParameterName")
+ public void drivePolar(double magnitude, double angle, double zRotation) {
+ if (!m_reported) {
+ HAL.report(tResourceType.kResourceType_RobotDrive, 4, tInstances.kRobotDrive2_MecanumPolar);
+ m_reported = true;
+ }
+
+ driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
+ magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
+ }
+
+ /**
+ * Gets if the power sent to the right side of the drivetrain is multipled by -1.
+ *
+ * @return true if the right side is inverted
+ */
+ public boolean isRightSideInverted() {
+ return m_rightSideInvertMultiplier == -1.0;
+ }
+
+ /**
+ * Sets if the power sent to the right side of the drivetrain should be multipled by -1.
+ *
+ * @param rightSideInverted true if right side power should be multipled by -1
+ */
+ public void setRightSideInverted(boolean rightSideInverted) {
+ m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+ }
+
+ @Override
+ public void stopMotor() {
+ m_frontLeftMotor.stopMotor();
+ m_frontRightMotor.stopMotor();
+ m_rearLeftMotor.stopMotor();
+ m_rearRightMotor.stopMotor();
+ feed();
+ }
+
+ @Override
+ public String getDescription() {
+ return "MecanumDrive";
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("MecanumDrive");
+ builder.setActuator(true);
+ builder.setSafeState(this::stopMotor);
+ builder.addDoubleProperty("Front Left Motor Speed",
+ m_frontLeftMotor::get,
+ m_frontLeftMotor::set);
+ builder.addDoubleProperty("Front Right Motor Speed",
+ () -> m_frontRightMotor.get() * m_rightSideInvertMultiplier,
+ value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier));
+ builder.addDoubleProperty("Rear Left Motor Speed",
+ m_rearLeftMotor::get,
+ m_rearLeftMotor::set);
+ builder.addDoubleProperty("Rear Right Motor Speed",
+ () -> m_rearRightMotor.get() * m_rightSideInvertMultiplier,
+ value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier));
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
new file mode 100644
index 0000000..2e47434
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+import edu.wpi.first.wpilibj.MotorSafety;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.SendableImpl;
+
+/**
+ * Common base class for drive platforms.
+ */
+public abstract class RobotDriveBase extends MotorSafety implements Sendable, AutoCloseable {
+ public static final double kDefaultDeadband = 0.02;
+ public static final double kDefaultMaxOutput = 1.0;
+
+ protected double m_deadband = kDefaultDeadband;
+ protected double m_maxOutput = kDefaultMaxOutput;
+
+ private final SendableImpl m_sendableImpl;
+
+ /**
+ * The location of a motor on the robot for the purpose of driving.
+ */
+ public enum MotorType {
+ kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3), kLeft(0),
+ kRight(1), kBack(2);
+
+ @SuppressWarnings("MemberName")
+ public final int value;
+
+ MotorType(int value) {
+ this.value = value;
+ }
+ }
+
+ /**
+ * RobotDriveBase constructor.
+ */
+ public RobotDriveBase() {
+ m_sendableImpl = new SendableImpl(true);
+
+ setSafetyEnabled(true);
+ setName("RobotDriveBase");
+ }
+
+ @Override
+ public void close() {
+ m_sendableImpl.close();
+ }
+
+ @Override
+ public final synchronized String getName() {
+ return m_sendableImpl.getName();
+ }
+
+ @Override
+ public final synchronized void setName(String name) {
+ m_sendableImpl.setName(name);
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into
+ */
+ protected final void setName(String moduleType, int channel) {
+ m_sendableImpl.setName(moduleType, channel);
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ */
+ protected final void setName(String moduleType, int moduleNumber, int channel) {
+ m_sendableImpl.setName(moduleType, moduleNumber, channel);
+ }
+
+ @Override
+ public final synchronized String getSubsystem() {
+ return m_sendableImpl.getSubsystem();
+ }
+
+ @Override
+ public final synchronized void setSubsystem(String subsystem) {
+ m_sendableImpl.setSubsystem(subsystem);
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ protected final void addChild(Object child) {
+ m_sendableImpl.addChild(child);
+ }
+
+ /**
+ * Sets the deadband applied to the drive inputs (e.g., joystick values).
+ *
+ * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
+ * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See
+ * {@link #applyDeadband}.
+ *
+ * @param deadband The deadband to set.
+ */
+ public void setDeadband(double deadband) {
+ m_deadband = deadband;
+ }
+
+ /**
+ * Configure the scaling factor for using drive methods with motor controllers in a mode other
+ * than PercentVbus or to limit the maximum output.
+ *
+ * <p>The default value is {@value #kDefaultMaxOutput}.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+ */
+ public void setMaxOutput(double maxOutput) {
+ m_maxOutput = maxOutput;
+ }
+
+ /**
+ * Feed the motor safety object. Resets the timer that will stop the motors if it completes.
+ *
+ * @see MotorSafety#feed()
+ */
+ public void feedWatchdog() {
+ feed();
+ }
+
+ @Override
+ public abstract void stopMotor();
+
+ @Override
+ public abstract String getDescription();
+
+ /**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+ protected double limit(double value) {
+ if (value > 1.0) {
+ return 1.0;
+ }
+ if (value < -1.0) {
+ return -1.0;
+ }
+ return value;
+ }
+
+ /**
+ * Returns 0.0 if the given value is within the specified range around zero. The remaining range
+ * between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ *
+ * @param value value to clip
+ * @param deadband range around zero
+ */
+ protected double applyDeadband(double value, double deadband) {
+ if (Math.abs(value) > deadband) {
+ if (value > 0.0) {
+ return (value - deadband) / (1.0 - deadband);
+ } else {
+ return (value + deadband) / (1.0 - deadband);
+ }
+ } else {
+ return 0.0;
+ }
+ }
+
+ /**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+ protected void normalize(double[] wheelSpeeds) {
+ double maxMagnitude = Math.abs(wheelSpeeds[0]);
+ for (int i = 1; i < wheelSpeeds.length; i++) {
+ double temp = Math.abs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) {
+ maxMagnitude = temp;
+ }
+ }
+ if (maxMagnitude > 1.0) {
+ for (int i = 0; i < wheelSpeeds.length; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
new file mode 100644
index 0000000..2b1f839
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.drive;
+
+/**
+ * This is a 2D vector struct that supports basic vector operations.
+ */
+@SuppressWarnings("MemberName")
+public class Vector2d {
+ public double x;
+ public double y;
+
+ public Vector2d() {}
+
+ @SuppressWarnings("ParameterName")
+ public Vector2d(double x, double y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * Rotate a vector in Cartesian space.
+ *
+ * @param angle angle in degrees by which to rotate vector counter-clockwise.
+ */
+ public void rotate(double angle) {
+ double cosA = Math.cos(angle * (Math.PI / 180.0));
+ double sinA = Math.sin(angle * (Math.PI / 180.0));
+ double[] out = new double[2];
+ out[0] = x * cosA - y * sinA;
+ out[1] = x * sinA + y * cosA;
+ x = out[0];
+ y = out[1];
+ }
+
+ /**
+ * Returns dot product of this vector with argument.
+ *
+ * @param vec Vector with which to perform dot product.
+ */
+ public double dot(Vector2d vec) {
+ return x * vec.x + y * vec.y;
+ }
+
+ /**
+ * Returns magnitude of vector.
+ */
+ public double magnitude() {
+ return Math.sqrt(x * x + y * y);
+ }
+
+ /**
+ * Returns scalar projection of this vector onto argument.
+ *
+ * @param vec Vector onto which to project this vector.
+ */
+ public double scalarProject(Vector2d vec) {
+ return dot(vec) / vec.magnitude();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
new file mode 100644
index 0000000..26312f8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.filters;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/**
+ * Superclass for filters.
+ */
+public abstract class Filter implements PIDSource {
+ private final PIDSource m_source;
+
+ public Filter(PIDSource source) {
+ m_source = source;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_source.setPIDSourceType(pidSource);
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_source.getPIDSourceType();
+ }
+
+ @Override
+ public abstract double pidGet();
+
+ /**
+ * Returns the current filter estimate without also inserting new data as pidGet() would do.
+ *
+ * @return The current filter estimate
+ */
+ public abstract double get();
+
+ /**
+ * Reset the filter state.
+ */
+ public abstract void reset();
+
+ /**
+ * Calls PIDGet() of source.
+ *
+ * @return Current value of source
+ */
+ protected double pidGetSource() {
+ return m_source.pidGet();
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
new file mode 100644
index 0000000..65e84b5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.filters;
+
+import java.util.Arrays;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.CircularBuffer;
+import edu.wpi.first.wpilibj.PIDSource;
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
+ * Static factory methods are provided to create commonly used types of filters.
+ *
+ * <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
+ * a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ * <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
+ * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
+ * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
+ * impact of these high frequency components. Likewise, a "high pass" filter gets rid of
+ * slow-moving signal components, letting you detect large changes more easily.
+ *
+ * <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
+ * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
+ * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
+ * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
+ * PID controller out of this class!
+ *
+ * <p>For more on filters, I highly recommend the following articles: http://en.wikipedia
+ * .org/wiki/Linear_filter http://en.wikipedia.org/wiki/Iir_filter http://en.wikipedia
+ * .org/wiki/Fir_filter
+ *
+ * <p>Note 1: PIDGet() should be called by the user on a known, regular period. You can set up a
+ * Notifier to do this (look at the WPILib PIDController class), or do it "inline" with code in a
+ * periodic function.
+ *
+ * <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
+ * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
+ * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
+ * to make sure PIDGet() gets called at the desired, constant frequency!
+ */
+public class LinearDigitalFilter extends Filter {
+ private static int instances;
+
+ private final CircularBuffer m_inputs;
+ private final CircularBuffer m_outputs;
+ private final double[] m_inputGains;
+ private final double[] m_outputGains;
+
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+ public LinearDigitalFilter(PIDSource source, double[] ffGains,
+ double[] fbGains) {
+ super(source);
+ m_inputs = new CircularBuffer(ffGains.length);
+ m_outputs = new CircularBuffer(fbGains.length);
+ m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+ m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+ instances++;
+ HAL.report(tResourceType.kResourceType_LinearFilter, instances);
+ }
+
+ /**
+ * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
+ * gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ * <p>This filter is stable for time constants greater than zero.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+ public static LinearDigitalFilter singlePoleIIR(PIDSource source,
+ double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {1.0 - gain};
+ double[] fbGains = {-gain};
+
+ return new LinearDigitalFilter(source, ffGains, fbGains);
+ }
+
+ /**
+ * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
+ * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ * <p>This filter is stable for time constants greater than zero.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+ public static LinearDigitalFilter highPass(PIDSource source,
+ double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {gain, -gain};
+ double[] fbGains = {-gain};
+
+ return new LinearDigitalFilter(source, ffGains, fbGains);
+ }
+
+ /**
+ * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
+ * x[0]).
+ *
+ * <p>This filter is always stable.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param taps The number of samples to average over. Higher = smoother but slower
+ * @throws IllegalArgumentException if number of taps is less than 1
+ */
+ public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
+ if (taps <= 0) {
+ throw new IllegalArgumentException("Number of taps was not at least 1");
+ }
+
+ double[] ffGains = new double[taps];
+ for (int i = 0; i < ffGains.length; i++) {
+ ffGains[i] = 1.0 / taps;
+ }
+
+ double[] fbGains = new double[0];
+
+ return new LinearDigitalFilter(source, ffGains, fbGains);
+ }
+
+ @Override
+ public double get() {
+ double retVal = 0.0;
+
+ // Calculate the new value
+ for (int i = 0; i < m_inputGains.length; i++) {
+ retVal += m_inputs.get(i) * m_inputGains[i];
+ }
+ for (int i = 0; i < m_outputGains.length; i++) {
+ retVal -= m_outputs.get(i) * m_outputGains[i];
+ }
+
+ return retVal;
+ }
+
+ @Override
+ public void reset() {
+ m_inputs.clear();
+ m_outputs.clear();
+ }
+
+ /**
+ * Calculates the next value of the filter.
+ *
+ * @return The filtered value at this step
+ */
+ @Override
+ public double pidGet() {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.addFirst(pidGetSource());
+
+ // Calculate the new value
+ for (int i = 0; i < m_inputGains.length; i++) {
+ retVal += m_inputs.get(i) * m_inputGains[i];
+ }
+ for (int i = 0; i < m_outputGains.length; i++) {
+ retVal -= m_outputs.get(i) * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.addFirst(retVal);
+
+ return retVal;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
new file mode 100644
index 0000000..81086d4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.interfaces;
+
+/**
+ * Interface for 3-axis accelerometers.
+ */
+public interface Accelerometer {
+ enum Range {
+ k2G, k4G, k8G, k16G
+ }
+
+ /**
+ * Common interface for setting the measuring range of an accelerometer.
+ *
+ * @param range The maximum acceleration, positive or negative, that the accelerometer will
+ * measure. Not all accelerometers support all ranges.
+ */
+ void setRange(Range range);
+
+ /**
+ * Common interface for getting the x axis acceleration.
+ *
+ * @return The acceleration along the x axis in g-forces
+ */
+ double getX();
+
+ /**
+ * Common interface for getting the y axis acceleration.
+ *
+ * @return The acceleration along the y axis in g-forces
+ */
+ double getY();
+
+ /**
+ * Common interface for getting the z axis acceleration.
+ *
+ * @return The acceleration along the z axis in g-forces
+ */
+ double getZ();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
new file mode 100644
index 0000000..4a5b2de
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.interfaces;
+
+/**
+ * Interface for yaw rate gyros.
+ */
+public interface Gyro extends AutoCloseable {
+ /**
+ * Calibrate the gyro by running for a number of samples and computing the center value. Then use
+ * the center value as the Accumulator center value for subsequent measurements. It's important to
+ * make sure that the robot is not moving while the centering calculations are in progress, this
+ * is typically done when the robot is first turned on while it's sitting at rest before the
+ * competition starts.
+ */
+ void calibrate();
+
+ /**
+ * Reset the gyro. Resets the gyro to a heading of zero. This can be used if there is significant
+ * drift in the gyro and it needs to be recalibrated after it has been running.
+ */
+ void reset();
+
+ /**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * <p>The angle is based on the current accumulator value corrected by the oversampling rate, the
+ * gyro type and the A/D calibration values. The angle is continuous, that is it will continue
+ * from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in
+ * the gyro output as it sweeps past from 360 to 0 on the second time around.
+ *
+ * <p>The angle is expected to increase as the gyro turns clockwise when looked
+ * at from the top. It needs to follow NED axis conventions in order to work
+ * properly with dependent control loops.
+ *
+ * <p>This heading is based on integration of the returned rate from the gyro.
+ *
+ * @return the current heading of the robot in degrees.
+ */
+ double getAngle();
+
+ /**
+ * Return the rate of rotation of the gyro.
+ *
+ * <p>The rate is based on the most recent reading of the gyro analog value
+ *
+ * <p>The rate is expected to be positive as the gyro turns clockwise when looked
+ * at from the top. It needs to follow NED axis conventions in order to work
+ * properly with dependent control loops.
+ *
+ * @return the current rate in degrees per second
+ */
+ double getRate();
+
+ /**
+ * Free the resources used by the gyro.
+ */
+ @Deprecated
+ void free();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
new file mode 100644
index 0000000..ea5c1e6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.interfaces;
+
+import edu.wpi.first.wpilibj.PIDSource;
+
+/**
+ * Interface for a Potentiometer.
+ */
+public interface Potentiometer extends PIDSource {
+ double get();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
new file mode 100644
index 0000000..1cdb4ab
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -0,0 +1,296 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.livewindow;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
+
+
+/**
+ * The LiveWindow class is the public interface for putting sensors and actuators on the
+ * LiveWindow.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class LiveWindow {
+ private static class Component {
+ Component(Sendable sendable, Sendable parent) {
+ m_sendable = sendable;
+ m_parent = parent;
+ }
+
+ final Sendable m_sendable;
+ Sendable m_parent;
+ final SendableBuilderImpl m_builder = new SendableBuilderImpl();
+ boolean m_firstTime = true;
+ boolean m_telemetryEnabled = true;
+ }
+
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private static final Map<Object, Component> components = new HashMap<>();
+ private static final NetworkTable liveWindowTable =
+ NetworkTableInstance.getDefault().getTable("LiveWindow");
+ private static final NetworkTable statusTable = liveWindowTable.getSubTable(".status");
+ private static final NetworkTableEntry enabledEntry = statusTable.getEntry("LW Enabled");
+ private static boolean startLiveWindow;
+ private static boolean liveWindowEnabled;
+ private static boolean telemetryEnabled = true;
+
+ private LiveWindow() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ public static synchronized boolean isEnabled() {
+ return liveWindowEnabled;
+ }
+
+ /**
+ * Set the enabled state of LiveWindow. If it's being enabled, turn off the scheduler and remove
+ * all the commands from the queue and enable all the components registered for LiveWindow. If
+ * it's being disabled, stop all the registered components and reenable the scheduler. TODO: add
+ * code to disable PID loops when enabling LiveWindow. The commands should reenable the PID loops
+ * themselves when they get rescheduled. This prevents arms from starting to move around, etc.
+ * after a period of adjusting them in LiveWindow mode.
+ */
+ public static synchronized void setEnabled(boolean enabled) {
+ if (liveWindowEnabled != enabled) {
+ startLiveWindow = enabled;
+ liveWindowEnabled = enabled;
+ updateValues(); // Force table generation now to make sure everything is defined
+ Scheduler scheduler = Scheduler.getInstance();
+ if (enabled) {
+ System.out.println("Starting live window mode.");
+ scheduler.disable();
+ scheduler.removeAll();
+ } else {
+ System.out.println("stopping live window mode.");
+ for (Component component : components.values()) {
+ component.m_builder.stopLiveWindowMode();
+ }
+ scheduler.enable();
+ }
+ enabledEntry.setBoolean(enabled);
+ }
+ }
+
+ /**
+ * The run method is called repeatedly to keep the values refreshed on the screen in test mode.
+ * @deprecated No longer required
+ */
+ @Deprecated
+ public static void run() {
+ updateValues();
+ }
+
+ /**
+ * Add a Sensor associated with the subsystem and with call it by the given name.
+ *
+ * @param subsystem The subsystem this component is part of.
+ * @param name The name of this component.
+ * @param component A LiveWindowSendable component that represents a sensor.
+ * @deprecated Use {@link Sendable#setName(String, String)} instead.
+ */
+ @Deprecated
+ public static synchronized void addSensor(String subsystem, String name, Sendable component) {
+ add(component);
+ component.setName(subsystem, name);
+ }
+
+ /**
+ * Add Sensor to LiveWindow. The components are shown with the type and channel like this: Gyro[1]
+ * for a gyro object connected to the first analog channel.
+ *
+ * @param moduleType A string indicating the type of the module used in the naming (above)
+ * @param channel The channel number the device is connected to
+ * @param component A reference to the object being added
+ * @deprecated Use {@link edu.wpi.first.wpilibj.SendableBase#setName(String, int)} instead.
+ */
+ @Deprecated
+ public static void addSensor(String moduleType, int channel, Sendable component) {
+ add(component);
+ component.setName("Ungrouped", moduleType + "[" + channel + "]");
+ }
+
+ /**
+ * Add an Actuator associated with the subsystem and with call it by the given name.
+ *
+ * @param subsystem The subsystem this component is part of.
+ * @param name The name of this component.
+ * @param component A LiveWindowSendable component that represents a actuator.
+ * @deprecated Use {@link Sendable#setName(String, String)} instead.
+ */
+ @Deprecated
+ public static synchronized void addActuator(String subsystem, String name, Sendable component) {
+ add(component);
+ component.setName(subsystem, name);
+ }
+
+ /**
+ * Add Actuator to LiveWindow. The components are shown with the module type, slot and channel
+ * like this: Servo[1,2] for a servo object connected to the first digital module and PWM port 2.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param channel The channel number the device is plugged into (usually PWM)
+ * @param component The reference to the object being added
+ * @deprecated Use {@link edu.wpi.first.wpilibj.SendableBase#setName(String, int)} instead.
+ */
+ @Deprecated
+ public static void addActuator(String moduleType, int channel, Sendable component) {
+ add(component);
+ component.setName("Ungrouped", moduleType + "[" + channel + "]");
+ }
+
+ /**
+ * Add Actuator to LiveWindow. The components are shown with the module type, slot and channel
+ * like this: Servo[1,2] for a servo object connected to the first digital module and PWM port 2.
+ *
+ * @param moduleType A string that defines the module name in the label for the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually PWM)
+ * @param component The reference to the object being added
+ * @deprecated Use {@link edu.wpi.first.wpilibj.SendableBase#setName(String, int, int)} instead.
+ */
+ @Deprecated
+ public static void addActuator(String moduleType, int moduleNumber, int channel,
+ Sendable component) {
+ add(component);
+ component.setName("Ungrouped", moduleType + "[" + moduleNumber + "," + channel + "]");
+ }
+
+ /**
+ * Add a component to the LiveWindow.
+ *
+ * @param sendable component to add
+ */
+ public static synchronized void add(Sendable sendable) {
+ components.putIfAbsent(sendable, new Component(sendable, null));
+ }
+
+ /**
+ * Add a child component to a component.
+ *
+ * @param parent parent component
+ * @param child child component
+ */
+ public static synchronized void addChild(Sendable parent, Object child) {
+ Component component = components.get(child);
+ if (component == null) {
+ component = new Component(null, parent);
+ components.put(child, component);
+ } else {
+ component.m_parent = parent;
+ }
+ component.m_telemetryEnabled = false;
+ }
+
+ /**
+ * Remove a component from the LiveWindow.
+ *
+ * @param sendable component to remove
+ */
+ public static synchronized void remove(Sendable sendable) {
+ Component component = components.remove(sendable);
+ if (component != null && isEnabled()) {
+ component.m_builder.stopLiveWindowMode();
+ }
+ }
+
+ /**
+ * Enable telemetry for a single component.
+ *
+ * @param sendable component
+ */
+ public static synchronized void enableTelemetry(Sendable sendable) {
+ // Re-enable global setting in case disableAllTelemetry() was called.
+ telemetryEnabled = true;
+ Component component = components.get(sendable);
+ if (component != null) {
+ component.m_telemetryEnabled = true;
+ }
+ }
+
+ /**
+ * Disable telemetry for a single component.
+ *
+ * @param sendable component
+ */
+ public static synchronized void disableTelemetry(Sendable sendable) {
+ Component component = components.get(sendable);
+ if (component != null) {
+ component.m_telemetryEnabled = false;
+ }
+ }
+
+ /**
+ * Disable ALL telemetry.
+ */
+ public static synchronized void disableAllTelemetry() {
+ telemetryEnabled = false;
+ for (Component component : components.values()) {
+ component.m_telemetryEnabled = false;
+ }
+ }
+
+ /**
+ * Tell all the sensors to update (send) their values.
+ *
+ * <p>Actuators are handled through callbacks on their value changing from the
+ * SmartDashboard widgets.
+ */
+ @SuppressWarnings("PMD.CyclomaticComplexity")
+ public static synchronized void updateValues() {
+ // Only do this if either LiveWindow mode or telemetry is enabled.
+ if (!liveWindowEnabled && !telemetryEnabled) {
+ return;
+ }
+
+ for (Component component : components.values()) {
+ if (component.m_sendable != null && component.m_parent == null
+ && (liveWindowEnabled || component.m_telemetryEnabled)) {
+ if (component.m_firstTime) {
+ // By holding off creating the NetworkTable entries, it allows the
+ // components to be redefined. This allows default sensor and actuator
+ // values to be created that are replaced with the custom names from
+ // users calling setName.
+ String name = component.m_sendable.getName();
+ if (name.isEmpty()) {
+ continue;
+ }
+ String subsystem = component.m_sendable.getSubsystem();
+ NetworkTable ssTable = liveWindowTable.getSubTable(subsystem);
+ NetworkTable table;
+ // Treat name==subsystem as top level of subsystem
+ if (name.equals(subsystem)) {
+ table = ssTable;
+ } else {
+ table = ssTable.getSubTable(name);
+ }
+ table.getEntry(".name").setString(name);
+ component.m_builder.setTable(table);
+ component.m_sendable.initSendable(component.m_builder);
+ ssTable.getEntry(".type").setString("LW Subsystem");
+
+ component.m_firstTime = false;
+ }
+
+ if (startLiveWindow) {
+ component.m_builder.startLiveWindowMode();
+ }
+ component.m_builder.updateTable();
+ }
+ }
+
+ startLiveWindow = false;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java
new file mode 100644
index 0000000..85d88c3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.livewindow;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Live Window Sendable is a special type of object sendable to the live window.
+ * @deprecated Use Sendable directly instead
+ */
+@Deprecated
+public interface LiveWindowSendable extends Sendable {
+ /**
+ * Update the table for this sendable object with the latest values.
+ */
+ void updateTable();
+
+ /**
+ * Start having this sendable object automatically respond to value changes reflect the value on
+ * the table.
+ */
+ void startLiveWindowMode();
+
+ /**
+ * Stop having this sendable object automatically respond to value changes.
+ */
+ void stopLiveWindowMode();
+
+ @Override
+ default String getName() {
+ return "";
+ }
+
+ @Override
+ default void setName(String name) {
+ }
+
+ @Override
+ default String getSubsystem() {
+ return "";
+ }
+
+ @Override
+ default void setSubsystem(String subsystem) {
+ }
+
+ @Override
+ default void initSendable(SendableBuilder builder) {
+ builder.setUpdateTable(this::updateTable);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/package.html b/wpilibj/src/main/java/edu/wpi/first/wpilibj/package.html
new file mode 100644
index 0000000..d11ae27
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/package.html
@@ -0,0 +1,30 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html>
+ <head>
+ <title>
+ WPI Robotics library
+ </title>
+ <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+ </head>
+ <body>
+ The WPI Robotics library (WPILibJ) is a set of Java classes that interfaces
+ to the hardware in the FRC control system and your robot. There are classes
+ to handle sensors, motors, the driver station, and a number of other
+ utility functions like timing and field management. The library is designed
+ to:
+ <ul>
+ <li>Deal with all the low level interfacing to these components so you
+ can concentrate on solving this year's "robot problem". This is a
+ philosophical decision to let you focus on the higher-level design of
+ your robot rather than deal with the details of the processor and the
+ operating system.
+ </li>
+ <li>Understand everything at all levels by making the full source code of
+ the library available. You can study (and modify) the algorithms used by
+ the gyro class for oversampling and integration of the input signal or
+ just ask the class for the current robot heading. You can work at any
+ level.
+ </li>
+ </ul>
+ </body>
+</html>
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
new file mode 100644
index 0000000..27c3e8c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * The types of layouts bundled with Shuffleboard.
+ *
+ * <pre>{@code
+ * ShuffleboardLayout myList = Shuffleboard.getTab("My Tab")
+ * .getLayout(BuiltinLayouts.kList, "My List");
+ * }</pre>
+ */
+public enum BuiltInLayouts implements LayoutType {
+ /**
+ * Groups components in a vertical list. New widgets added to the layout will be placed at the
+ * bottom of the list.
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Label position</td><td>String</td><td>"BOTTOM"</td>
+ * <td>The position of component labels inside the grid. One of
+ * {@code ["TOP", "LEFT", "BOTTOM", "RIGHT", "HIDDEN"}</td></tr>
+ * </table>
+ */
+ kList("List Layout"),
+
+ /**
+ * Groups components in an <i>n</i> x <i>m</i> grid. Grid layouts default to 3x3.
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Number of columns</td><td>Number</td><td>3</td><td>Must be in the range [1,15]</td>
+ * </tr>
+ * <tr><td>Number of rows</td><td>Number</td><td>3</td><td>Must be in the range [1,15]</td></tr>
+ * <tr>
+ * <td>Label position</td>
+ * <td>String</td>
+ * <td>"BOTTOM"</td>
+ * <td>The position of component labels inside the grid.
+ * One of {@code ["TOP", "LEFT", "BOTTOM", "RIGHT", "HIDDEN"}</td>
+ * </tr>
+ * </table>
+ */
+ kGrid("Grid Layout"),
+ ;
+
+ private final String m_layoutName;
+
+ BuiltInLayouts(String layoutName) {
+ m_layoutName = layoutName;
+ }
+
+ @Override
+ public String getLayoutName() {
+ return m_layoutName;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
new file mode 100644
index 0000000..14742e5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
@@ -0,0 +1,399 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.wpilibj.interfaces.Accelerometer.Range;
+
+/**
+ * The types of the widgets bundled with Shuffleboard.
+ *
+ * <p>For example, setting a number to be displayed with a slider:
+ * <pre>{@code
+ * NetworkTableEntry example = Shuffleboard.getTab("My Tab")
+ * .add("My Number", 0)
+ * .withWidget(BuiltInWidgets.kNumberSlider)
+ * .withProperties(Map.of("min", 0, "max", 1))
+ * .getEntry();
+ * }</pre>
+ *
+ * <p>Each value in this enum goes into detail on what data types that widget can support, as well
+ * as the custom properties that widget uses.
+ */
+public enum BuiltInWidgets implements WidgetType {
+ /**
+ * Displays a value with a simple text field.
+ * <br>Supported types:
+ * <ul>
+ * <li>String</li>
+ * <li>Number</li>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kTextView("Text View"),
+ /**
+ * Displays a number with a controllable slider.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>-1.0</td><td>The minimum value of the slider</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>1.0</td><td>The maximum value of the slider</td></tr>
+ * <tr><td>Block increment</td><td>Number</td><td>0.0625</td>
+ * <td>How much to move the slider by with the arrow keys</td></tr>
+ * </table>
+ */
+ kNumberSlider("Number Slider"),
+ /**
+ * Displays a number with a view-only bar.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>-1.0</td><td>The minimum value of the bar</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>1.0</td><td>The maximum value of the bar</td></tr>
+ * <tr><td>Center</td><td>Number</td><td>0</td><td>The center ("zero") value of the bar</td></tr>
+ * </table>
+ */
+ kNumberBar("Number Bar"),
+ /**
+ * Displays a number with a view-only dial. Displayed values are rounded to the nearest integer.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>0</td><td>The minimum value of the dial</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>100</td><td>The maximum value of the dial</td></tr>
+ * <tr><td>Show value</td><td>Boolean</td><td>true</td>
+ * <td>Whether or not to show the value as text</td></tr>
+ * </table>
+ */
+ kDial("Simple Dial"),
+ /**
+ * Displays a number with a graph. <strong>NOTE:</strong> graphs can be taxing on the computer
+ * running the dashboard. Keep the number of visible data points to a minimum. Making the widget
+ * smaller also helps with performance, but may cause the graph to become difficult to read.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * <li>Number array</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Visible time</td><td>Number</td><td>30</td>
+ * <td>How long, in seconds, should past data be visible for</td></tr>
+ * </table>
+ */
+ kGraph("Graph"),
+ /**
+ * Displays a boolean value as a large colored box.
+ * <br>Supported types:
+ * <ul>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Color when true</td><td>Color</td><td>"green"</td>
+ * <td>Can be specified as a string ({@code "#00FF00"}) or a rgba integer ({@code 0x00FF0000})
+ * </td></tr>
+ * <tr><td>Color when false</td><td>Color</td><td>"red"</td>
+ * <td>Can be specified as a string or a number</td></tr>
+ * </table>
+ */
+ kBooleanBox("Boolean Box"),
+ /**
+ * Displays a boolean with a large interactive toggle button.
+ * <br>Supported types:
+ * <ul>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kToggleButton("Toggle Button"),
+ /**
+ * Displays a boolean with a fixed-size toggle switch.
+ * <br>Supported types:
+ * <ul>
+ * <li>Boolean</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kToggleSwitch("Toggle Switch"),
+ /**
+ * Displays an analog input or a raw number with a number bar.
+ * <br>Supported types:
+ * <ul>
+ * <li>Number</li>
+ * <li>{@link edu.wpi.first.wpilibj.AnalogInput}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>0</td><td>The minimum value of the bar</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>5</td><td>The maximum value of the bar</td></tr>
+ * <tr><td>Center</td><td>Number</td><td>0</td><td>The center ("zero") value of the bar</td></tr>
+ * <tr><td>Orientation</td><td>String</td><td>"HORIZONTAL"</td>
+ * <td>The orientation of the bar. One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
+ * <tr><td>Number of tick marks</td><td>Number</td><td>5</td>
+ * <td>The number of discrete ticks on the bar</td></tr>
+ * </table>
+ */
+ kVoltageView("Voltage View"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.PowerDistributionPanel PowerDistributionPanel}.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.PowerDistributionPanel}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Show voltage and current values</td><td>Boolean</td><td>true</td>
+ * <td>Whether or not to display the voltage and current draw</td></tr>
+ * </table>
+ */
+ kPowerDistributionPanel("PDP"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with
+ * a dropdown combo box with a list of options.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kComboBoxChooser("ComboBox Chooser"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with
+ * a toggle button for each available option.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kSplitButtonChooser("Split Button Chooser"),
+ /**
+ * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed, total travelled
+ * distance, and its distance per tick.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.Encoder}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kEncoder("Encoder"),
+ /**
+ * Displays a {@link edu.wpi.first.wpilibj.SpeedController SpeedController}. The speed controller
+ * will be controllable from the dashboard when test mode is enabled, but will otherwise be
+ * view-only.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.PWMSpeedController}</li>
+ * <li>{@link edu.wpi.first.wpilibj.DMC60}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Jaguar}</li>
+ * <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}</li>
+ * <li>{@link edu.wpi.first.wpilibj.PWMVictorSPX}</li>
+ * <li>{@link edu.wpi.first.wpilibj.SD540}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Spark}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Talon}</li>
+ * <li>{@link edu.wpi.first.wpilibj.Victor}</li>
+ * <li>{@link edu.wpi.first.wpilibj.VictorSP}</li>
+ * <li>{@link edu.wpi.first.wpilibj.SpeedControllerGroup}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Orientation</td><td>String</td><td>"HORIZONTAL"</td>
+ * <td>One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
+ * </table>
+ */
+ kSpeedController("Speed Controller"),
+ /**
+ * Displays a command with a toggle button. Pressing the button will start the command, and the
+ * button will automatically release when the command completes.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.command.Command}</li>
+ * <li>{@link edu.wpi.first.wpilibj.command.CommandGroup}</li>
+ * <li>Any custom subclass of {@code Command} or {@code CommandGroup}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kCommand("Command"),
+ /**
+ * Displays a PID command with a checkbox and an editor for the PIDF constants. Selecting the
+ * checkbox will start the command, and the checkbox will automatically deselect when the command
+ * completes.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.command.PIDCommand}</li>
+ * <li>Any custom subclass of {@code PIDCommand}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kPIDCommand("PID Command"),
+ /**
+ * Displays a PID controller with an editor for the PIDF constants and a toggle switch for
+ * enabling and disabling the controller.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.PIDController}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kPIDController("PID Controller"),
+ /**
+ * Displays an accelerometer with a number bar displaying the magnitude of the acceleration and
+ * text displaying the exact value.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.AnalogAccelerometer}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Min</td><td>Number</td><td>-1</td>
+ * <td>The minimum acceleration value to display</td></tr>
+ * <tr><td>Max</td><td>Number</td><td>1</td>
+ * <td>The maximum acceleration value to display</td></tr>
+ * <tr><td>Show text</td><td>Boolean</td><td>true</td>
+ * <td>Show or hide the acceleration values</td></tr>
+ * <tr><td>Precision</td><td>Number</td><td>2</td>
+ * <td>How many numbers to display after the decimal point</td></tr>
+ * <tr><td>Show tick marks</td><td>Boolean</td><td>false</td>
+ * <td>Show or hide the tick marks on the number bars</td></tr>
+ * </table>
+ */
+ kAccelerometer("Accelerometer"),
+ /**
+ * Displays a 3-axis accelerometer with a number bar for each axis' accleration.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.ADXL345_I2C}</li>
+ * <li>{@link edu.wpi.first.wpilibj.ADXL345_SPI}</li>
+ * <li>{@link edu.wpi.first.wpilibj.ADXL362}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Range</td><td>{@link Range}</td><td>k16G</td><td>The accelerometer range</td></tr>
+ * <tr><td>Show value</td><td>Boolean</td><td>true</td>
+ * <td>Show or hide the acceleration values</td></tr>
+ * <tr><td>Precision</td><td>Number</td><td>2</td>
+ * <td>How many numbers to display after the decimal point</td></tr>
+ * <tr><td>Show tick marks</td><td>Boolean</td><td>false</td>
+ * <td>Show or hide the tick marks on the number bars</td></tr>
+ * </table>
+ */
+ k3AxisAccelerometer("3-Axis Accelerometer"),
+ /**
+ * Displays a gyro with a dial from 0 to 360 degrees.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}</li>
+ * <li>{@link edu.wpi.first.wpilibj.AnalogGyro}</li>
+ * <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Major tick spacing</td><td>Number</td><td>45</td><td>Degrees</td></tr>
+ * <tr><td>Starting angle</td><td>Number</td><td>180</td>
+ * <td>How far to rotate the entire dial, in degrees</td></tr>
+ * <tr><td>Show tick mark ring</td><td>Boolean</td><td>true</td></tr>
+ * </table>
+ */
+ kGyro("Gyro"),
+ /**
+ * Displays a relay with toggle buttons for each supported mode (off, on, forward, reverse).
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.Relay}</li>
+ * </ul>
+ * <br>This widget has no custom properties.
+ */
+ kRelay("Relay"),
+ /**
+ * Displays a differential drive with a widget that displays the speed of each side of the
+ * drivebase and a vector for the direction and rotation of the drivebase. The widget will be
+ * controllable if the robot is in test mode.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.drive.DifferentialDrive}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Number of wheels</td><td>Number</td><td>4</td><td>Must be a positive even integer
+ * </td></tr>
+ * <tr><td>Wheel diameter</td><td>Number</td><td>80</td><td>Pixels</td></tr>
+ * <tr><td>Show velocity vectors</td><td>Boolean</td><td>true</td></tr>
+ * </table>
+ */
+ kDifferentialDrive("Differential Drivebase"),
+ /**
+ * Displays a mecanum drive with a widget that displays the speed of each wheel, and vectors for
+ * the direction and rotation of the drivebase. The widget will be controllable if the robot is
+ * in test mode.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.first.wpilibj.drive.MecanumDrive}</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Show velocity vectors</td><td>Boolean</td><td>true</td></tr>
+ * </table>
+ */
+ kMecanumDrive("Mecanum Drivebase"),
+ /**
+ * Displays a camera stream.
+ * <br>Supported types:
+ * <ul>
+ * <li>{@link edu.wpi.cscore.VideoSource} (as long as it is streaming on an MJPEG server)</li>
+ * </ul>
+ * <br>Custom properties:
+ * <table>
+ * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
+ * <tr><td>Show crosshair</td><td>Boolean</td><td>true</td>
+ * <td>Show or hide a crosshair on the image</td></tr>
+ * <tr><td>Crosshair color</td><td>Color</td><td>"white"</td>
+ * <td>Can be a string or a rgba integer</td></tr>
+ * <tr><td>Show controls</td><td>Boolean</td><td>true</td><td>Show or hide the stream controls
+ * </td></tr>
+ * <tr><td>Rotation</td><td>String</td><td>"NONE"</td>
+ * <td>Rotates the displayed image. One of {@code ["NONE", "QUARTER_CW", "QUARTER_CCW", "HALF"]}
+ * </td></tr>
+ * </table>
+ */
+ kCameraStream("Camera Stream"),
+ ;
+
+ private final String m_widgetName;
+
+ BuiltInWidgets(String widgetName) {
+ this.m_widgetName = widgetName;
+ }
+
+ @Override
+ public String getWidgetName() {
+ return m_widgetName;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
new file mode 100644
index 0000000..d56fde8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
+
+/**
+ * A Shuffleboard widget that handles a {@link Sendable} object such as a speed controller or
+ * sensor.
+ */
+public final class ComplexWidget extends ShuffleboardWidget<ComplexWidget> {
+ private final Sendable m_sendable;
+ private SendableBuilderImpl m_builder;
+
+ ComplexWidget(ShuffleboardContainer parent, String title, Sendable sendable) {
+ super(parent, title);
+ m_sendable = sendable;
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ if (m_builder == null) {
+ m_builder = new SendableBuilderImpl();
+ m_builder.setTable(parentTable.getSubTable(getTitle()));
+ m_sendable.initSendable(m_builder);
+ m_builder.startListeners();
+ }
+ m_builder.updateTable();
+ }
+
+ /**
+ * Enables user control of this widget in the Shuffleboard application. This method is
+ * package-private to prevent users from enabling control themselves. Has no effect if the
+ * sendable is not marked as an actuator with {@link SendableBuilder#setActuator}.
+ */
+ void enableIfActuator() {
+ if (m_builder.isActuator()) {
+ m_builder.startLiveWindowMode();
+ }
+ }
+
+ /**
+ * Disables user control of this widget in the Shuffleboard application. This method is
+ * package-private to prevent users from enabling control themselves. Has no effect if the
+ * sendable is not marked as an actuator with {@link SendableBuilder#setActuator}.
+ */
+ void disableIfActuator() {
+ if (m_builder.isActuator()) {
+ m_builder.stopLiveWindowMode();
+ }
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
new file mode 100644
index 0000000..9cb63a0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.NoSuchElementException;
+import java.util.Objects;
+import java.util.Set;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * A helper class for Shuffleboard containers to handle common child operations.
+ */
+final class ContainerHelper {
+ private final ShuffleboardContainer m_container;
+ private final Set<String> m_usedTitles = new HashSet<>();
+ private final List<ShuffleboardComponent<?>> m_components = new ArrayList<>();
+ private final Map<String, ShuffleboardLayout> m_layouts = new LinkedHashMap<>();
+
+ ContainerHelper(ShuffleboardContainer container) {
+ m_container = container;
+ }
+
+ List<ShuffleboardComponent<?>> getComponents() {
+ return m_components;
+ }
+
+ ShuffleboardLayout getLayout(String title, String type) {
+ if (!m_layouts.containsKey(title)) {
+ ShuffleboardLayout layout = new ShuffleboardLayout(m_container, type, title);
+ m_components.add(layout);
+ m_layouts.put(title, layout);
+ }
+ return m_layouts.get(title);
+ }
+
+ ShuffleboardLayout getLayout(String title) {
+ ShuffleboardLayout layout = m_layouts.get(title);
+ if (layout == null) {
+ throw new NoSuchElementException("No layout has been defined with the title '" + title + "'");
+ }
+ return layout;
+ }
+
+ ComplexWidget add(String title, Sendable sendable) {
+ checkTitle(title);
+ ComplexWidget widget = new ComplexWidget(m_container, title, sendable);
+ m_components.add(widget);
+ return widget;
+ }
+
+ ComplexWidget add(Sendable sendable) throws IllegalArgumentException {
+ if (sendable.getName() == null || sendable.getName().isEmpty()) {
+ throw new IllegalArgumentException("Sendable must have a name");
+ }
+ return add(sendable.getName(), sendable);
+ }
+
+ SimpleWidget add(String title, Object defaultValue) {
+ Objects.requireNonNull(title, "Title cannot be null");
+ Objects.requireNonNull(defaultValue, "Default value cannot be null");
+ checkTitle(title);
+ checkNtType(defaultValue);
+
+ SimpleWidget widget = new SimpleWidget(m_container, title);
+ m_components.add(widget);
+ widget.getEntry().setDefaultValue(defaultValue);
+ return widget;
+ }
+
+ private static void checkNtType(Object data) {
+ if (!NetworkTableEntry.isValidDataType(data)) {
+ throw new IllegalArgumentException(
+ "Cannot add data of type " + data.getClass().getName() + " to Shuffleboard");
+ }
+ }
+
+ private void checkTitle(String title) {
+ if (m_usedTitles.contains(title)) {
+ throw new IllegalArgumentException("Title is already in use: " + title);
+ }
+ m_usedTitles.add(title);
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
new file mode 100644
index 0000000..08f9712
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * The importance of an event marker in Shuffleboard. The exact meaning of each importance level is
+ * up for interpretation on a team-to-team basis, but users should follow the general guidelines
+ * of the various importance levels. The examples given are for reference and may be ignored or
+ * considered to be more or less important from team to team.
+ */
+public enum EventImportance {
+ // Maintainer note: this enum is mirrored in WPILibC and in Shuffleboard
+ // Modifying the enum or enum strings requires a corresponding change to the C++ enum
+ // and the enum in Shuffleboard
+
+ /**
+ * A trivial event such as a change in command state.
+ */
+ kTrivial("TRIVIAL"),
+
+ /**
+ * A low importance event such as acquisition of a game piece.
+ */
+ kLow("LOW"),
+
+ /**
+ * A "normal" importance event, such as a transition from autonomous mode to teleoperated control.
+ */
+ kNormal("NORMAL"),
+
+ /**
+ * A high-importance event such as scoring a game piece.
+ */
+ kHigh("HIGH"),
+
+ /**
+ * A critically important event such as a brownout, component failure, or software deadlock.
+ */
+ kCritical("CRITICAL");
+
+ private final String m_simpleName;
+
+ EventImportance(String simpleName) {
+ m_simpleName = simpleName;
+ }
+
+ public String getSimpleName() {
+ return m_simpleName;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
new file mode 100644
index 0000000..2d7b565
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * Represents the type of a layout in Shuffleboard. Using this is preferred over specifying raw
+ * strings, to avoid typos and having to know or look up the exact string name for a desired layout.
+ *
+ * @see BuiltInWidgets the built-in widget types
+ */
+public interface LayoutType {
+ /**
+ * Gets the string type of the layout as defined by that layout in Shuffleboard.
+ */
+ String getLayoutName();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
new file mode 100644
index 0000000..9f04274
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * Controls Shuffleboard recordings via NetworkTables.
+ */
+final class RecordingController {
+ private static final String kRecordingTableName = "/Shuffleboard/.recording/";
+ private static final String kRecordingControlKey = kRecordingTableName + "RecordData";
+ private static final String kRecordingFileNameFormatKey = kRecordingTableName + "FileNameFormat";
+ private static final String kEventMarkerTableName = kRecordingTableName + "events";
+
+ private final NetworkTableEntry m_recordingControlEntry;
+ private final NetworkTableEntry m_recordingFileNameFormatEntry;
+ private final NetworkTable m_eventsTable;
+
+ RecordingController(NetworkTableInstance ntInstance) {
+ m_recordingControlEntry = ntInstance.getEntry(kRecordingControlKey);
+ m_recordingFileNameFormatEntry = ntInstance.getEntry(kRecordingFileNameFormatKey);
+ m_eventsTable = ntInstance.getTable(kEventMarkerTableName);
+ }
+
+ public void startRecording() {
+ m_recordingControlEntry.setBoolean(true);
+ }
+
+ public void stopRecording() {
+ m_recordingControlEntry.setBoolean(false);
+ }
+
+ public void setRecordingFileNameFormat(String format) {
+ m_recordingFileNameFormatEntry.setString(format);
+ }
+
+ public void clearRecordingFileNameFormat() {
+ m_recordingFileNameFormatEntry.delete();
+ }
+
+ public void addEventMarker(String name, String description, EventImportance importance) {
+ if (name == null || name.isEmpty()) {
+ DriverStation.reportError(
+ "Shuffleboard event name was not specified", true);
+ return;
+ }
+
+ if (importance == null) {
+ DriverStation.reportError(
+ "Shuffleboard event importance was null", true);
+ return;
+ }
+
+ String eventDescription = description == null ? "" : description;
+
+ m_eventsTable.getSubTable(name)
+ .getEntry("Info")
+ .setStringArray(new String[]{eventDescription, importance.getSimpleName()});
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
new file mode 100644
index 0000000..7aa93ee
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.Map;
+import java.util.WeakHashMap;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A wrapper to make video sources sendable and usable from Shuffleboard.
+ */
+public final class SendableCameraWrapper extends SendableBase {
+ private static final String kProtocol = "camera_server://";
+
+ private static Map<VideoSource, SendableCameraWrapper> m_wrappers = new WeakHashMap<>();
+
+ private final String m_uri;
+
+ /**
+ * Creates a new sendable wrapper. Private constructor to avoid direct instantiation with
+ * multiple wrappers floating around for the same camera.
+ *
+ * @param source the source to wrap
+ */
+ private SendableCameraWrapper(VideoSource source) {
+ super(false);
+ String name = source.getName();
+ setName(name);
+ m_uri = kProtocol + name;
+ }
+
+ /**
+ * Gets a sendable wrapper object for the given video source, creating the wrapper if one does
+ * not already exist for the source.
+ *
+ * @param source the video source to wrap
+ * @return a sendable wrapper object for the video source, usable in Shuffleboard via
+ * {@link ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
+ */
+ public static SendableCameraWrapper wrap(VideoSource source) {
+ return m_wrappers.computeIfAbsent(source, SendableCameraWrapper::new);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.addStringProperty(".ShuffleboardURI", () -> m_uri, null);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
new file mode 100644
index 0000000..fbce24a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
@@ -0,0 +1,201 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * The Shuffleboard class provides a mechanism with which data can be added and laid out in the
+ * Shuffleboard dashboard application from a robot program. Tabs and layouts can be specified, as
+ * well as choosing which widgets to display with and setting properties of these widgets; for
+ * example, programmers can specify a specific {@code boolean} value to be displayed with a toggle
+ * button instead of the default colored box, or set custom colors for that box.
+ *
+ * <p>For example, displaying a boolean entry with a toggle button:
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ * .add("My Boolean", false)
+ * .withWidget("Toggle Button")
+ * .getEntry();
+ * }</pre>
+ *
+ * Changing the colors of the boolean box:
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ * .add("My Boolean", false)
+ * .withWidget("Boolean Box")
+ * .withProperties(Map.of("colorWhenTrue", "green", "colorWhenFalse", "maroon"))
+ * .getEntry();
+ * }</pre>
+ *
+ * Specifying a parent layout. Note that the layout type must <i>always</i> be specified, even if
+ * the layout has already been generated by a previously defined entry.
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ * .getLayout("List", "Example List")
+ * .add("My Boolean", false)
+ * .withWidget("Toggle Button")
+ * .getEntry();
+ * }</pre>
+ * </p>
+ *
+ * <p>Teams are encouraged to set up shuffleboard layouts at the start of the robot program.</p>
+ */
+public final class Shuffleboard {
+ /**
+ * The name of the base NetworkTable into which all Shuffleboard data will be added.
+ */
+ public static final String kBaseTableName = "/Shuffleboard";
+
+ private static final ShuffleboardRoot root =
+ new ShuffleboardInstance(NetworkTableInstance.getDefault());
+ private static final RecordingController recordingController =
+ new RecordingController(NetworkTableInstance.getDefault());
+
+ // TODO usage reporting
+
+ private Shuffleboard() {
+ throw new UnsupportedOperationException("This is a utility class and cannot be instantiated");
+ }
+
+ /**
+ * Updates all the values in Shuffleboard. Iterative and timed robots are pre-configured to call
+ * this method in the main robot loop; teams using custom robot base classes, or subclass
+ * SampleRobot, should make sure to call this repeatedly to keep data on the dashboard up to date.
+ */
+ public static void update() {
+ root.update();
+ }
+
+ /**
+ * Gets the Shuffleboard tab with the given title, creating it if it does not already exist.
+ *
+ * @param title the title of the tab
+ * @return the tab with the given title
+ */
+ public static ShuffleboardTab getTab(String title) {
+ return root.getTab(title);
+ }
+
+ /**
+ * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i>
+ * is the number of tabs in the dashboard at the time this method is called.
+ *
+ * @param index the index of the tab to select
+ */
+ public static void selectTab(int index) {
+ root.selectTab(index);
+ }
+
+ /**
+ * Selects the tab in the dashboard with the given title.
+ *
+ * @param title the title of the tab to select
+ */
+ public static void selectTab(String title) {
+ root.selectTab(title);
+ }
+
+ /**
+ * Enables user control of widgets containing actuators: speed controllers, relays, etc. This
+ * should only be used when the robot is in test mode. IterativeRobotBase and SampleRobot are
+ * both configured to call this method when entering test mode; most users should not need to use
+ * this method directly.
+ */
+ public static void enableActuatorWidgets() {
+ root.enableActuatorWidgets();
+ }
+
+ /**
+ * Disables user control of widgets containing actuators. For safety reasons, actuators should
+ * only be controlled while in test mode. IterativeRobotBase and SampleRobot are both configured
+ * to call this method when exiting in test mode; most users should not need to use
+ * this method directly.
+ */
+ public static void disableActuatorWidgets() {
+ update(); // Need to update to make sure the sendable builders are initialized
+ root.disableActuatorWidgets();
+ }
+
+ /**
+ * Starts data recording on the dashboard. Has no effect if recording is already in progress.
+ *
+ * @see #stopRecording()
+ */
+ public static void startRecording() {
+ recordingController.startRecording();
+ }
+
+ /**
+ * Stops data recording on the dashboard. Has no effect if no recording is in progress.
+ *
+ * @see #startRecording()
+ */
+ public static void stopRecording() {
+ recordingController.stopRecording();
+ }
+
+ /**
+ * Sets the file name format for new recording files to use. If recording is in progress when this
+ * method is called, it will continue to use the same file. New recordings will use the format.
+ *
+ * <p>To avoid recording files overwriting each other, make sure to use unique recording file
+ * names. File name formats accept templates for inserting the date and time when the recording
+ * started with the {@code ${date}} and {@code ${time}} templates, respectively. For example,
+ * the default format is {@code "recording-${time}"} and recording files created with it will have
+ * names like {@code "recording-2018.01.15.sbr"}. Users are <strong>strongly</strong> recommended
+ * to use the {@code ${time}} template to ensure unique file names.
+ * </p>
+ *
+ * @param format the format for the
+ * @see #clearRecordingFileNameFormat()
+ */
+ public static void setRecordingFileNameFormat(String format) {
+ recordingController.setRecordingFileNameFormat(format);
+ }
+
+ /**
+ * Clears the custom name format for recording files. New recordings will use the default format.
+ *
+ * @see #setRecordingFileNameFormat(String)
+ */
+ public static void clearRecordingFileNameFormat() {
+ recordingController.clearRecordingFileNameFormat();
+ }
+
+ /**
+ * Notifies Shuffleboard of an event. Events can range from as trivial as a change in a command
+ * state to as critical as a total power loss or component failure. If Shuffleboard is recording,
+ * the event will also be recorded.
+ *
+ * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then
+ * no event will be sent and an error will be printed to the driver station.
+ *
+ * @param name the name of the event
+ * @param description a description of the event
+ * @param importance the importance of the event
+ */
+ public static void addEventMarker(String name, String description, EventImportance importance) {
+ recordingController.addEventMarker(name, description, importance);
+ }
+
+ /**
+ * Notifies Shuffleboard of an event. Events can range from as trivial as a change in a command
+ * state to as critical as a total power loss or component failure. If Shuffleboard is recording,
+ * the event will also be recorded.
+ *
+ * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then
+ * no event will be sent and an error will be printed to the driver station.
+ *
+ * @param name the name of the event
+ * @param importance the importance of the event
+ */
+ public static void addEventMarker(String name, EventImportance importance) {
+ addEventMarker(name, "", importance);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
new file mode 100644
index 0000000..ba87d82
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.Map;
+import java.util.Objects;
+
+import edu.wpi.first.networktables.NetworkTable;
+
+/**
+ * A generic component in Shuffleboard.
+ *
+ * @param <C> the self type
+ */
+public abstract class ShuffleboardComponent<C extends ShuffleboardComponent<C>>
+ implements ShuffleboardValue {
+ private final ShuffleboardContainer m_parent;
+ private final String m_title;
+ private String m_type;
+ private Map<String, Object> m_properties;
+ private boolean m_metadataDirty = true;
+ private int m_column = -1;
+ private int m_row = -1;
+ private int m_width = -1;
+ private int m_height = -1;
+
+ protected ShuffleboardComponent(ShuffleboardContainer parent, String title, String type) {
+ m_parent = Objects.requireNonNull(parent, "Parent cannot be null");
+ m_title = Objects.requireNonNull(title, "Title cannot be null");
+ m_type = type;
+ }
+
+ protected ShuffleboardComponent(ShuffleboardContainer parent, String title) {
+ this(parent, title, null);
+ }
+
+ public final ShuffleboardContainer getParent() {
+ return m_parent;
+ }
+
+ protected final void setType(String type) {
+ m_type = type;
+ m_metadataDirty = true;
+ }
+
+ public final String getType() {
+ return m_type;
+ }
+
+ @Override
+ public final String getTitle() {
+ return m_title;
+ }
+
+ /**
+ * Gets the custom properties for this component. May be null.
+ */
+ final Map<String, Object> getProperties() {
+ return m_properties;
+ }
+
+ /**
+ * Sets custom properties for this component. Property names are case- and whitespace-insensitive
+ * (capitalization and spaces do not matter).
+ *
+ * @param properties the properties for this component
+ * @return this component
+ */
+ public final C withProperties(Map<String, Object> properties) {
+ m_properties = properties;
+ m_metadataDirty = true;
+ return (C) this;
+ }
+
+ /**
+ * Sets the position of this component in the tab. This has no effect if this component is inside
+ * a layout.
+ *
+ * <p>If the position of a single component is set, it is recommended to set the positions of
+ * <i>all</i> components inside a tab to prevent Shuffleboard from automatically placing another
+ * component there before the one with the specific position is sent.
+ *
+ * @param columnIndex the column in the tab to place this component
+ * @param rowIndex the row in the tab to place this component
+ * @return this component
+ */
+ public final C withPosition(int columnIndex, int rowIndex) {
+ m_column = columnIndex;
+ m_row = rowIndex;
+ m_metadataDirty = true;
+ return (C) this;
+ }
+
+ /**
+ * Sets the size of this component in the tab. This has no effect if this component is inside a
+ * layout.
+ *
+ * @param width how many columns wide the component should be
+ * @param height how many rows high the component should be
+ * @return this component
+ */
+ public final C withSize(int width, int height) {
+ m_width = width;
+ m_height = height;
+ m_metadataDirty = true;
+ return (C) this;
+ }
+
+ protected final void buildMetadata(NetworkTable metaTable) {
+ if (!m_metadataDirty) {
+ return;
+ }
+ // Component type
+ if (getType() == null) {
+ metaTable.getEntry("PreferredComponent").delete();
+ } else {
+ metaTable.getEntry("PreferredComponent").forceSetString(getType());
+ }
+
+ // Tile size
+ if (m_width <= 0 || m_height <= 0) {
+ metaTable.getEntry("Size").delete();
+ } else {
+ metaTable.getEntry("Size").setDoubleArray(new double[]{m_width, m_height});
+ }
+
+ // Tile position
+ if (m_column < 0 || m_row < 0) {
+ metaTable.getEntry("Position").delete();
+ } else {
+ metaTable.getEntry("Position").setDoubleArray(new double[]{m_column, m_row});
+ }
+
+ // Custom properties
+ if (getProperties() != null) {
+ NetworkTable propTable = metaTable.getSubTable("Properties");
+ getProperties().forEach((name, value) -> propTable.getEntry(name).setValue(value));
+ }
+ m_metadataDirty = false;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
new file mode 100644
index 0000000..a322af1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.List;
+import java.util.NoSuchElementException;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * Common interface for objects that can contain shuffleboard components.
+ */
+public interface ShuffleboardContainer extends ShuffleboardValue {
+
+ /**
+ * Gets the components that are direct children of this container.
+ */
+ List<ShuffleboardComponent<?>> getComponents();
+
+ /**
+ * Gets the layout with the given type and title, creating it if it does not already exist at the
+ * time this method is called. Note: this method should only be used to use a layout type that
+ * is not already built into Shuffleboard. To use a layout built into Shuffleboard, use
+ * {@link #getLayout(String, LayoutType)} and the layouts in {@link BuiltInLayouts}.
+ *
+ * @param title the title of the layout
+ * @param type the type of the layout, eg "List Layout" or "Grid Layout"
+ * @return the layout
+ * @see #getLayout(String, LayoutType)
+ */
+ ShuffleboardLayout getLayout(String title, String type);
+
+ /**
+ * Gets the layout with the given type and title, creating it if it does not already exist at the
+ * time this method is called.
+ *
+ * @param title the title of the layout
+ * @param layoutType the type of the layout, eg "List" or "Grid"
+ * @return the layout
+ */
+ default ShuffleboardLayout getLayout(String title, LayoutType layoutType) {
+ return getLayout(title, layoutType.getLayoutName());
+ }
+
+ /**
+ * Gets the already-defined layout in this container with the given title.
+ *
+ * <pre>{@code
+ * Shuffleboard.getTab("Example Tab")
+ * .getLayout("My Layout", BuiltInLayouts.kList);
+ *
+ * // Later...
+ * Shuffleboard.getTab("Example Tab")
+ * .getLayout("My Layout");
+ * }</pre>
+ *
+ * @param title the title of the layout to get
+ * @return the layout with the given title
+ * @throws NoSuchElementException if no layout has yet been defined with the given title
+ */
+ ShuffleboardLayout getLayout(String title) throws NoSuchElementException;
+
+ /**
+ * Adds a widget to this container to display the given sendable.
+ *
+ * @param title the title of the widget
+ * @param sendable the sendable to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ ComplexWidget add(String title, Sendable sendable) throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container to display the given video stream.
+ *
+ * @param title the title of the widget
+ * @param video the video stream to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ */
+ default ComplexWidget add(String title, VideoSource video) throws IllegalArgumentException {
+ return add(title, SendableCameraWrapper.wrap(video));
+ }
+
+ /**
+ * Adds a widget to this container to display the given sendable.
+ *
+ * @param sendable the sendable to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title, or if the sendable's name has not been specified
+ */
+ ComplexWidget add(Sendable sendable);
+
+ /**
+ * Adds a widget to this container to display the given video stream.
+ *
+ * @param video the video to display
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the same
+ * title as the video source
+ */
+ default ComplexWidget add(VideoSource video) {
+ return add(SendableCameraWrapper.wrap(video));
+ }
+
+ /**
+ * Adds a widget to this container to display the given data.
+ *
+ * @param title the title of the widget
+ * @param defaultValue the default value of the widget
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+ */
+ SimpleWidget add(String title, Object defaultValue) throws IllegalArgumentException;
+
+ /**
+ * Adds a widget to this container to display a simple piece of data. Unlike
+ * {@link #add(String, Object)}, the value in the widget will be saved on the robot and will be
+ * used when the robot program next starts rather than {@code defaultValue}.
+ *
+ * @param title the title of the widget
+ * @param defaultValue the default value of the widget
+ * @return a widget to display the sendable data
+ * @throws IllegalArgumentException if a widget already exists in this container with the given
+ * title
+ * @see #add(String, Object) add(String title, Object defaultValue)
+ */
+ default SimpleWidget addPersistent(String title, Object defaultValue)
+ throws IllegalArgumentException {
+ SimpleWidget widget = add(title, defaultValue);
+ widget.getEntry().setPersistent();
+ return widget;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
new file mode 100644
index 0000000..7018da0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.LinkedHashMap;
+import java.util.Map;
+import java.util.Objects;
+import java.util.function.Consumer;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+final class ShuffleboardInstance implements ShuffleboardRoot {
+ private final Map<String, ShuffleboardTab> m_tabs = new LinkedHashMap<>();
+
+ private boolean m_tabsChanged = false; // NOPMD redundant field initializer
+ private final NetworkTable m_rootTable;
+ private final NetworkTable m_rootMetaTable;
+ private final NetworkTableEntry m_selectedTabEntry;
+
+ ShuffleboardInstance(NetworkTableInstance ntInstance) {
+ Objects.requireNonNull(ntInstance, "NetworkTable instance cannot be null");
+ m_rootTable = ntInstance.getTable(Shuffleboard.kBaseTableName);
+ m_rootMetaTable = m_rootTable.getSubTable(".metadata");
+ m_selectedTabEntry = m_rootMetaTable.getEntry("Selected");
+ }
+
+ @Override
+ public ShuffleboardTab getTab(String title) {
+ Objects.requireNonNull(title, "Tab title cannot be null");
+ if (!m_tabs.containsKey(title)) {
+ m_tabs.put(title, new ShuffleboardTab(this, title));
+ m_tabsChanged = true;
+ }
+ return m_tabs.get(title);
+ }
+
+ @Override
+ public void update() {
+ if (m_tabsChanged) {
+ String[] tabTitles = m_tabs.values()
+ .stream()
+ .map(ShuffleboardTab::getTitle)
+ .toArray(String[]::new);
+ m_rootMetaTable.getEntry("Tabs").forceSetStringArray(tabTitles);
+ m_tabsChanged = false;
+ }
+ for (ShuffleboardTab tab : m_tabs.values()) {
+ String title = tab.getTitle();
+ tab.buildInto(m_rootTable, m_rootMetaTable.getSubTable(title));
+ }
+ }
+
+ @Override
+ public void enableActuatorWidgets() {
+ applyToAllComplexWidgets(ComplexWidget::enableIfActuator);
+ }
+
+ @Override
+ public void disableActuatorWidgets() {
+ applyToAllComplexWidgets(ComplexWidget::disableIfActuator);
+ }
+
+ @Override
+ public void selectTab(int index) {
+ m_selectedTabEntry.forceSetDouble(index);
+ }
+
+ @Override
+ public void selectTab(String title) {
+ m_selectedTabEntry.forceSetString(title);
+ }
+
+ /**
+ * Applies the function {@code func} to all complex widgets in this root, regardless of how they
+ * are nested.
+ *
+ * @param func the function to apply to all complex widgets
+ */
+ private void applyToAllComplexWidgets(Consumer<ComplexWidget> func) {
+ for (ShuffleboardTab tab : m_tabs.values()) {
+ apply(tab, func);
+ }
+ }
+
+ /**
+ * Applies the function {@code func} to all complex widgets in {@code container}. Helper method
+ * for {@link #applyToAllComplexWidgets}.
+ */
+ private void apply(ShuffleboardContainer container, Consumer<ComplexWidget> func) {
+ for (ShuffleboardComponent<?> component : container.getComponents()) {
+ if (component instanceof ComplexWidget) {
+ func.accept((ComplexWidget) component);
+ }
+ if (component instanceof ShuffleboardContainer) {
+ apply((ShuffleboardContainer) component, func);
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
new file mode 100644
index 0000000..2a9d425
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.List;
+import java.util.NoSuchElementException;
+import java.util.Objects;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * A layout in a Shuffleboard tab. Layouts can contain widgets and other layouts.
+ */
+public class ShuffleboardLayout extends ShuffleboardComponent<ShuffleboardLayout>
+ implements ShuffleboardContainer {
+ private final ContainerHelper m_helper = new ContainerHelper(this);
+
+ ShuffleboardLayout(ShuffleboardContainer parent, String name, String type) {
+ super(parent, Objects.requireNonNull(type, "Layout type must be specified"), name);
+ }
+
+ @Override
+ public List<ShuffleboardComponent<?>> getComponents() {
+ return m_helper.getComponents();
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title, String type) {
+ return m_helper.getLayout(title, type);
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title) throws NoSuchElementException {
+ return m_helper.getLayout(title);
+ }
+
+ @Override
+ public ComplexWidget add(String title, Sendable sendable) throws IllegalArgumentException {
+ return m_helper.add(title, sendable);
+ }
+
+ @Override
+ public ComplexWidget add(Sendable sendable) throws IllegalArgumentException {
+ return m_helper.add(sendable);
+ }
+
+ @Override
+ public SimpleWidget add(String title, Object defaultValue) throws IllegalArgumentException {
+ return m_helper.add(title, defaultValue);
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ NetworkTable table = parentTable.getSubTable(getTitle());
+ table.getEntry(".type").setString("ShuffleboardLayout");
+ for (ShuffleboardComponent<?> component : getComponents()) {
+ component.buildInto(table, metaTable.getSubTable(component.getTitle()));
+ }
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
new file mode 100644
index 0000000..a32af69
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * The root of the data placed in Shuffleboard. It contains the tabs, but no data is placed
+ * directly in the root.
+ *
+ * <p>This class is package-private to minimize API surface area.
+ */
+interface ShuffleboardRoot {
+
+ /**
+ * Gets the tab with the given title, creating it if it does not already exist.
+ *
+ * @param title the title of the tab
+ * @return the tab with the given title
+ */
+ ShuffleboardTab getTab(String title);
+
+ /**
+ * Updates all tabs.
+ */
+ void update();
+
+ /**
+ * Enables all widgets in Shuffleboard that offer user control over actuators.
+ */
+ void enableActuatorWidgets();
+
+ /**
+ * Disables all widgets in Shuffleboard that offer user control over actuators.
+ */
+ void disableActuatorWidgets();
+
+ /**
+ * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i>
+ * is the number of tabs in the dashboard at the time this method is called.
+ *
+ * @param index the index of the tab to select
+ */
+ void selectTab(int index);
+
+ /**
+ * Selects the tab in the dashboard with the given title.
+ *
+ * @param title the title of the tab to select
+ */
+ void selectTab(String title);
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
new file mode 100644
index 0000000..aec70e2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.List;
+import java.util.NoSuchElementException;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * Represents a tab in the Shuffleboard dashboard. Widgets can be added to the tab with
+ * {@link #add(Sendable)}, {@link #add(String, Object)}, and {@link #add(String, Sendable)}. Widgets
+ * can also be added to layouts with {@link #getLayout(String, String)}; layouts can be nested
+ * arbitrarily deep (note that too many levels may make deeper components unusable).
+ */
+public final class ShuffleboardTab implements ShuffleboardContainer {
+ private final ContainerHelper m_helper = new ContainerHelper(this);
+ private final ShuffleboardRoot m_root;
+ private final String m_title;
+
+ ShuffleboardTab(ShuffleboardRoot root, String title) {
+ m_root = root;
+ m_title = title;
+ }
+
+ @Override
+ public String getTitle() {
+ return m_title;
+ }
+
+ ShuffleboardRoot getRoot() {
+ return m_root;
+ }
+
+ @Override
+ public List<ShuffleboardComponent<?>> getComponents() {
+ return m_helper.getComponents();
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title, String type) {
+ return m_helper.getLayout(title, type);
+ }
+
+ @Override
+ public ShuffleboardLayout getLayout(String title) throws NoSuchElementException {
+ return m_helper.getLayout(title);
+ }
+
+ @Override
+ public ComplexWidget add(String title, Sendable sendable) {
+ return m_helper.add(title, sendable);
+ }
+
+ @Override
+ public ComplexWidget add(Sendable sendable) throws IllegalArgumentException {
+ return m_helper.add(sendable);
+ }
+
+ @Override
+ public SimpleWidget add(String title, Object defaultValue) {
+ return m_helper.add(title, defaultValue);
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ NetworkTable tabTable = parentTable.getSubTable(m_title);
+ tabTable.getEntry(".type").setString("ShuffleboardTab");
+ for (ShuffleboardComponent<?> component : m_helper.getComponents()) {
+ component.buildInto(tabTable, metaTable.getSubTable(component.getTitle()));
+ }
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
new file mode 100644
index 0000000..8bfa6c3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+
+interface ShuffleboardValue {
+
+ /**
+ * Gets the title of this Shuffleboard value.
+ */
+ String getTitle();
+
+ /**
+ * Builds the entries for this value.
+ *
+ * @param parentTable the table containing all the data for the parent. Values that require a
+ * complex entry or table structure should call {@code
+ * parentTable.getSubTable(getTitle())} to get the table to put data into.
+ * Values that only use a single entry should call {@code
+ * parentTable.getEntry(getTitle())} to get that entry.
+ * @param metaTable the table containing all the metadata for this value and its sub-values
+ */
+ void buildInto(NetworkTable parentTable, NetworkTable metaTable);
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
new file mode 100644
index 0000000..3dc6622
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * Abstract superclass for widgets.
+ *
+ * <p>This class is package-private to minimize API surface area.
+ *
+ * @param <W> the self type
+ */
+abstract class ShuffleboardWidget<W extends ShuffleboardWidget<W>>
+ extends ShuffleboardComponent<W> {
+
+ ShuffleboardWidget(ShuffleboardContainer parent, String title) {
+ super(parent, title);
+ }
+
+ /**
+ * Sets the type of widget used to display the data. If not set, the default widget type will be
+ * used.
+ *
+ * @param widgetType the type of the widget used to display the data
+ * @return this widget object
+ * @see BuiltInWidgets
+ */
+ public final W withWidget(WidgetType widgetType) {
+ return withWidget(widgetType.getWidgetName());
+ }
+
+ /**
+ * Sets the type of widget used to display the data. If not set, the default widget type will be
+ * used. This method should only be used to use a widget that does not come built into
+ * Shuffleboard (i.e. one that comes with a custom or thrid-party plugin). To use a widget that
+ * is built into Shuffleboard, use {@link #withWidget(WidgetType)} and {@link BuiltInWidgets}.
+ *
+ * @param widgetType the type of the widget used to display the data
+ * @return this widget object
+ */
+ public final W withWidget(String widgetType) {
+ setType(widgetType);
+ return (W) this;
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
new file mode 100644
index 0000000..c645baa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+/**
+ * A Shuffleboard widget that handles a single data point such as a number or string.
+ */
+public final class SimpleWidget extends ShuffleboardWidget<SimpleWidget> {
+ private NetworkTableEntry m_entry;
+
+ SimpleWidget(ShuffleboardContainer parent, String title) {
+ super(parent, title);
+ }
+
+ /**
+ * Gets the NetworkTable entry that contains the data for this widget.
+ */
+ public NetworkTableEntry getEntry() {
+ if (m_entry == null) {
+ forceGenerate();
+ }
+ return m_entry;
+ }
+
+ @Override
+ public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+ buildMetadata(metaTable);
+ if (m_entry == null) {
+ m_entry = parentTable.getEntry(getTitle());
+ }
+ }
+
+ private void forceGenerate() {
+ ShuffleboardContainer parent = getParent();
+ while (parent instanceof ShuffleboardLayout) {
+ parent = ((ShuffleboardLayout) parent).getParent();
+ }
+ ShuffleboardTab tab = (ShuffleboardTab) parent;
+ tab.getRoot().update();
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
new file mode 100644
index 0000000..1e8242f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+/**
+ * Represents the type of a widget in Shuffleboard. Using this is preferred over specifying raw
+ * strings, to avoid typos and having to know or look up the exact string name for a desired widget.
+ *
+ * @see BuiltInWidgets the built-in widget types
+ */
+public interface WidgetType {
+ /**
+ * Gets the string type of the widget as defined by that widget in Shuffleboard.
+ */
+ String getWidgetName();
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java
new file mode 100644
index 0000000..35a8b54
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableValue;
+
+public interface SendableBuilder {
+ /**
+ * Set the string representation of the named data type that will be used
+ * by the smart dashboard for this sendable.
+ *
+ * @param type data type
+ */
+ void setSmartDashboardType(String type);
+
+ /**
+ * Set a flag indicating if this sendable should be treated as an actuator.
+ * By default this flag is false.
+ *
+ * @param value true if actuator, false if not
+ */
+ void setActuator(boolean value);
+
+ /**
+ * Set the function that should be called to set the Sendable into a safe
+ * state. This is called when entering and exiting Live Window mode.
+ *
+ * @param func function
+ */
+ void setSafeState(Runnable func);
+
+ /**
+ * Set the function that should be called to update the network table
+ * for things other than properties. Note this function is not passed
+ * the network table object; instead it should use the entry handles
+ * returned by getEntry().
+ *
+ * @param func function
+ */
+ void setUpdateTable(Runnable func);
+
+ /**
+ * Add a property without getters or setters. This can be used to get
+ * entry handles for the function called by setUpdateTable().
+ *
+ * @param key property name
+ * @return Network table entry
+ */
+ NetworkTableEntry getEntry(String key);
+
+ /**
+ * Represents an operation that accepts a single boolean-valued argument and
+ * returns no result. This is the primitive type specialization of Consumer
+ * for boolean. Unlike most other functional interfaces, BooleanConsumer is
+ * expected to operate via side-effects.
+ *
+ * <p>This is a functional interface whose functional method is accept(boolean).
+ */
+ @FunctionalInterface
+ interface BooleanConsumer {
+ /**
+ * Performs the operation on the given value.
+ * @param value the value
+ */
+ void accept(boolean value);
+ }
+
+ /**
+ * Add a boolean property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter);
+
+ /**
+ * Add a double property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter);
+
+ /**
+ * Add a string property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter);
+
+ /**
+ * Add a boolean array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addBooleanArrayProperty(String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter);
+
+ /**
+ * Add a double array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addDoubleArrayProperty(String key, Supplier<double[]> getter, Consumer<double[]> setter);
+
+ /**
+ * Add a string array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addStringArrayProperty(String key, Supplier<String[]> getter, Consumer<String[]> setter);
+
+ /**
+ * Add a raw property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter);
+
+ /**
+ * Add a NetworkTableValue property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ void addValueProperty(String key, Supplier<NetworkTableValue> getter,
+ Consumer<NetworkTableValue> setter);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
new file mode 100644
index 0000000..aec5a27
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
@@ -0,0 +1,393 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Function;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableValue;
+
+@SuppressWarnings("PMD.TooManyMethods")
+public class SendableBuilderImpl implements SendableBuilder {
+ private static class Property {
+ Property(NetworkTable table, String key) {
+ m_entry = table.getEntry(key);
+ }
+
+ @Override
+ @SuppressWarnings("NoFinalizer")
+ protected synchronized void finalize() {
+ stopListener();
+ }
+
+ void startListener() {
+ if (m_entry.isValid() && m_listener == 0 && m_createListener != null) {
+ m_listener = m_createListener.apply(m_entry);
+ }
+ }
+
+ void stopListener() {
+ if (m_entry.isValid() && m_listener != 0) {
+ m_entry.removeListener(m_listener);
+ m_listener = 0;
+ }
+ }
+
+ final NetworkTableEntry m_entry;
+ int m_listener;
+ Consumer<NetworkTableEntry> m_update;
+ Function<NetworkTableEntry, Integer> m_createListener;
+ }
+
+ private final List<Property> m_properties = new ArrayList<>();
+ private Runnable m_safeState;
+ private Runnable m_updateTable;
+ private NetworkTable m_table;
+ private NetworkTableEntry m_controllableEntry;
+ private boolean m_actuator;
+
+ /**
+ * Set the network table. Must be called prior to any Add* functions being
+ * called.
+ * @param table Network table
+ */
+ public void setTable(NetworkTable table) {
+ m_table = table;
+ m_controllableEntry = table.getEntry(".controllable");
+ }
+
+ /**
+ * Get the network table.
+ * @return The network table
+ */
+ public NetworkTable getTable() {
+ return m_table;
+ }
+
+ /**
+ * Return whether this sendable should be treated as an actuator.
+ * @return True if actuator, false if not.
+ */
+ public boolean isActuator() {
+ return m_actuator;
+ }
+
+ /**
+ * Update the network table values by calling the getters for all properties.
+ */
+ public void updateTable() {
+ for (Property property : m_properties) {
+ if (property.m_update != null) {
+ property.m_update.accept(property.m_entry);
+ }
+ }
+ if (m_updateTable != null) {
+ m_updateTable.run();
+ }
+ }
+
+ /**
+ * Hook setters for all properties.
+ */
+ public void startListeners() {
+ for (Property property : m_properties) {
+ property.startListener();
+ }
+ if (m_controllableEntry != null) {
+ m_controllableEntry.setBoolean(true);
+ }
+ }
+
+ /**
+ * Unhook setters for all properties.
+ */
+ public void stopListeners() {
+ for (Property property : m_properties) {
+ property.stopListener();
+ }
+ if (m_controllableEntry != null) {
+ m_controllableEntry.setBoolean(false);
+ }
+ }
+
+ /**
+ * Start LiveWindow mode by hooking the setters for all properties. Also
+ * calls the safeState function if one was provided.
+ */
+ public void startLiveWindowMode() {
+ if (m_safeState != null) {
+ m_safeState.run();
+ }
+ startListeners();
+ }
+
+ /**
+ * Stop LiveWindow mode by unhooking the setters for all properties. Also
+ * calls the safeState function if one was provided.
+ */
+ public void stopLiveWindowMode() {
+ stopListeners();
+ if (m_safeState != null) {
+ m_safeState.run();
+ }
+ }
+
+ /**
+ * Set the string representation of the named data type that will be used
+ * by the smart dashboard for this sendable.
+ *
+ * @param type data type
+ */
+ @Override
+ public void setSmartDashboardType(String type) {
+ m_table.getEntry(".type").setString(type);
+ }
+
+ /**
+ * Set a flag indicating if this sendable should be treated as an actuator.
+ * By default this flag is false.
+ *
+ * @param value true if actuator, false if not
+ */
+ @Override
+ public void setActuator(boolean value) {
+ m_table.getEntry(".actuator").setBoolean(value);
+ m_actuator = value;
+ }
+
+ /**
+ * Set the function that should be called to set the Sendable into a safe
+ * state. This is called when entering and exiting Live Window mode.
+ *
+ * @param func function
+ */
+ @Override
+ public void setSafeState(Runnable func) {
+ m_safeState = func;
+ }
+
+ /**
+ * Set the function that should be called to update the network table
+ * for things other than properties. Note this function is not passed
+ * the network table object; instead it should use the entry handles
+ * returned by getEntry().
+ *
+ * @param func function
+ */
+ @Override
+ public void setUpdateTable(Runnable func) {
+ m_updateTable = func;
+ }
+
+ /**
+ * Add a property without getters or setters. This can be used to get
+ * entry handles for the function called by setUpdateTable().
+ *
+ * @param key property name
+ * @return Network table entry
+ */
+ @Override
+ public NetworkTableEntry getEntry(String key) {
+ return m_table.getEntry(key);
+ }
+
+ /**
+ * Add a boolean property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setBoolean(getter.getAsBoolean());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isBoolean()) {
+ setter.accept(event.value.getBoolean());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a double property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setDouble(getter.getAsDouble());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isDouble()) {
+ setter.accept(event.value.getDouble());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a string property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setString(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isString()) {
+ setter.accept(event.value.getString());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a boolean array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addBooleanArrayProperty(String key, Supplier<boolean[]> getter,
+ Consumer<boolean[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setBooleanArray(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isBooleanArray()) {
+ setter.accept(event.value.getBooleanArray());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a double array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addDoubleArrayProperty(String key, Supplier<double[]> getter,
+ Consumer<double[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setDoubleArray(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isDoubleArray()) {
+ setter.accept(event.value.getDoubleArray());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a string array property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addStringArrayProperty(String key, Supplier<String[]> getter,
+ Consumer<String[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setStringArray(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isStringArray()) {
+ setter.accept(event.value.getStringArray());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a raw property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setRaw(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ if (event.value.isRaw()) {
+ setter.accept(event.value.getRaw());
+ }
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+
+ /**
+ * Add a NetworkTableValue property.
+ *
+ * @param key property name
+ * @param getter getter function (returns current value)
+ * @param setter setter function (sets new value)
+ */
+ @Override
+ public void addValueProperty(String key, Supplier<NetworkTableValue> getter,
+ Consumer<NetworkTableValue> setter) {
+ Property property = new Property(m_table, key);
+ if (getter != null) {
+ property.m_update = entry -> entry.setValue(getter.get());
+ }
+ if (setter != null) {
+ property.m_createListener = entry -> entry.addListener(event -> {
+ setter.accept(event.value);
+ }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+ }
+ m_properties.add(property);
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
new file mode 100644
index 0000000..5859705
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.command.Command;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * The {@link SendableChooser} class is a useful tool for presenting a selection of options to the
+ * {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
+ * this by putting every possible {@link Command} you want to run as an autonomous into a {@link
+ * SendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
+ * on the laptop. Once autonomous starts, simply ask the {@link SendableChooser} what the selected
+ * value is.
+ *
+ * @param <V> The type of the values to be stored
+ */
+public class SendableChooser<V> extends SendableBase {
+ /**
+ * The key for the default value.
+ */
+ private static final String DEFAULT = "default";
+ /**
+ * The key for the selected option.
+ */
+ private static final String SELECTED = "selected";
+ /**
+ * The key for the active option.
+ */
+ private static final String ACTIVE = "active";
+ /**
+ * The key for the option array.
+ */
+ private static final String OPTIONS = "options";
+ /**
+ * The key for the instance number.
+ */
+ private static final String INSTANCE = ".instance";
+ /**
+ * A map linking strings to the objects the represent.
+ */
+ @SuppressWarnings("PMD.LooseCoupling")
+ private final LinkedHashMap<String, V> m_map = new LinkedHashMap<>();
+ private String m_defaultChoice = "";
+ private final int m_instance;
+ private static final AtomicInteger s_instances = new AtomicInteger();
+
+ /**
+ * Instantiates a {@link SendableChooser}.
+ */
+ public SendableChooser() {
+ super(false);
+ m_instance = s_instances.getAndIncrement();
+ }
+
+ /**
+ * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
+ * object will appear as the given name.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ public void addOption(String name, V object) {
+ m_map.put(name, object);
+ }
+
+ /**
+ * Adds the given object to the list of options.
+ *
+ * @deprecated Use {@link #addOption(String, Object)} instead.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ @Deprecated
+ public void addObject(String name, V object) {
+ addOption(name, object);
+ }
+
+ /**
+ * Adds the given object to the list of options and marks it as the default. Functionally, this is
+ * very close to {@link #addOption(String, Object)} except that it will use this as the default
+ * option if none other is explicitly selected.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ public void setDefaultOption(String name, V object) {
+ requireNonNull(name, "Provided name was null");
+
+ m_defaultChoice = name;
+ addOption(name, object);
+ }
+
+ /**
+ * Adds the given object to the list of options and marks it as the default.
+ *
+ * @deprecated Use {@link #setDefaultOption(String, Object)} instead.
+ *
+ * @param name the name of the option
+ * @param object the option
+ */
+ @Deprecated
+ public void addDefault(String name, V object) {
+ setDefaultOption(name, object);
+ }
+
+ /**
+ * Returns the selected option. If there is none selected, it will return the default. If there is
+ * none selected and no default, then it will return {@code null}.
+ *
+ * @return the option selected
+ */
+ public V getSelected() {
+ m_mutex.lock();
+ try {
+ if (m_selected != null) {
+ return m_map.get(m_selected);
+ } else {
+ return m_map.get(m_defaultChoice);
+ }
+ } finally {
+ m_mutex.unlock();
+ }
+ }
+
+ private String m_selected;
+ private final List<NetworkTableEntry> m_activeEntries = new ArrayList<>();
+ private final ReentrantLock m_mutex = new ReentrantLock();
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("String Chooser");
+ builder.getEntry(INSTANCE).setDouble(m_instance);
+ builder.addStringProperty(DEFAULT, () -> {
+ return m_defaultChoice;
+ }, null);
+ builder.addStringArrayProperty(OPTIONS, () -> {
+ return m_map.keySet().toArray(new String[0]);
+ }, null);
+ builder.addStringProperty(ACTIVE, () -> {
+ m_mutex.lock();
+ try {
+ if (m_selected != null) {
+ return m_selected;
+ } else {
+ return m_defaultChoice;
+ }
+ } finally {
+ m_mutex.unlock();
+ }
+ }, null);
+ m_mutex.lock();
+ try {
+ m_activeEntries.add(builder.getEntry(ACTIVE));
+ } finally {
+ m_mutex.unlock();
+ }
+ builder.addStringProperty(SELECTED, null, val -> {
+ m_mutex.lock();
+ try {
+ m_selected = val;
+ for (NetworkTableEntry entry : m_activeEntries) {
+ entry.setString(val);
+ }
+ } finally {
+ m_mutex.unlock();
+ }
+ });
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
new file mode 100644
index 0000000..290fe33
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
@@ -0,0 +1,533 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.nio.ByteBuffer;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Set;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.NamedSendable;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on
+ * the laptop.
+ *
+ * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the
+ * laptop. Users can put values into and get values from the SmartDashboard.
+ */
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+public class SmartDashboard {
+ /**
+ * The {@link NetworkTable} used by {@link SmartDashboard}.
+ */
+ private static final NetworkTable table =
+ NetworkTableInstance.getDefault().getTable("SmartDashboard");
+
+ private static class Data {
+ Data(Sendable sendable) {
+ m_sendable = sendable;
+ }
+
+ final Sendable m_sendable;
+ final SendableBuilderImpl m_builder = new SendableBuilderImpl();
+ }
+
+ /**
+ * A table linking tables in the SmartDashboard to the {@link Sendable} objects they
+ * came from.
+ */
+ @SuppressWarnings("PMD.UseConcurrentHashMap")
+ private static final Map<String, Data> tablesToData = new HashMap<>();
+
+ static {
+ HAL.report(tResourceType.kResourceType_SmartDashboard, 0);
+ }
+
+ private SmartDashboard() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Maps the specified key to the specified value in this table. The key can not be null. The value
+ * can be retrieved by calling the get method with a key that is equal to the original key.
+ *
+ * @param key the key
+ * @param data the value
+ * @throws IllegalArgumentException If key is null
+ */
+ public static synchronized void putData(String key, Sendable data) {
+ Data sddata = tablesToData.get(key);
+ if (sddata == null || sddata.m_sendable != data) {
+ if (sddata != null) {
+ sddata.m_builder.stopListeners();
+ }
+ sddata = new Data(data);
+ tablesToData.put(key, sddata);
+ NetworkTable dataTable = table.getSubTable(key);
+ sddata.m_builder.setTable(dataTable);
+ data.initSendable(sddata.m_builder);
+ sddata.m_builder.updateTable();
+ sddata.m_builder.startListeners();
+ dataTable.getEntry(".name").setString(key);
+ }
+ }
+
+ /**
+ * Maps the specified key (where the key is the name of the {@link NamedSendable}
+ * to the specified value in this table. The value can be retrieved by
+ * calling the get method with a key that is equal to the original key.
+ *
+ * @param value the value
+ * @throws IllegalArgumentException If key is null
+ */
+ public static void putData(Sendable value) {
+ putData(value.getName(), value);
+ }
+
+ /**
+ * Returns the value at the specified key.
+ *
+ * @param key the key
+ * @return the value
+ * @throws IllegalArgumentException if the key is null
+ */
+ public static synchronized Sendable getData(String key) {
+ Data data = tablesToData.get(key);
+ if (data == null) {
+ throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
+ } else {
+ return data.m_sendable;
+ }
+ }
+
+ /**
+ * Gets the entry for the specified key.
+ * @param key the key name
+ * @return Network table entry.
+ */
+ public static NetworkTableEntry getEntry(String key) {
+ return table.getEntry(key);
+ }
+
+ /**
+ * Checks the table and tells if it contains the specified key.
+ *
+ * @param key the key to search for
+ * @return true if the table as a value assigned to the given key
+ */
+ public static boolean containsKey(String key) {
+ return table.containsKey(key);
+ }
+
+ /**
+ * Get the keys stored in the SmartDashboard table of NetworkTables.
+ *
+ * @param types bitmask of types; 0 is treated as a "don't care".
+ * @return keys currently in the table
+ */
+ public static Set<String> getKeys(int types) {
+ return table.getKeys(types);
+ }
+
+ /**
+ * Get the keys stored in the SmartDashboard table of NetworkTables.
+ *
+ * @return keys currently in the table.
+ */
+ public static Set<String> getKeys() {
+ return table.getKeys();
+ }
+
+ /**
+ * Makes a key's value persistent through program restarts.
+ * The key cannot be null.
+ *
+ * @param key the key name
+ */
+ public static void setPersistent(String key) {
+ getEntry(key).setPersistent();
+ }
+
+ /**
+ * Stop making a key's value persistent through program restarts.
+ * The key cannot be null.
+ *
+ * @param key the key name
+ */
+ public static void clearPersistent(String key) {
+ getEntry(key).clearPersistent();
+ }
+
+ /**
+ * Returns whether the value is persistent through program restarts.
+ * The key cannot be null.
+ *
+ * @param key the key name
+ * @return True if the value is persistent.
+ */
+ public static boolean isPersistent(String key) {
+ return getEntry(key).isPersistent();
+ }
+
+ /**
+ * Sets flags on the specified key in this table. The key can
+ * not be null.
+ *
+ * @param key the key name
+ * @param flags the flags to set (bitmask)
+ */
+ public static void setFlags(String key, int flags) {
+ getEntry(key).setFlags(flags);
+ }
+
+ /**
+ * Clears flags on the specified key in this table. The key can
+ * not be null.
+ *
+ * @param key the key name
+ * @param flags the flags to clear (bitmask)
+ */
+ public static void clearFlags(String key, int flags) {
+ getEntry(key).clearFlags(flags);
+ }
+
+ /**
+ * Returns the flags for the specified key.
+ *
+ * @param key the key name
+ * @return the flags, or 0 if the key is not defined
+ */
+ public static int getFlags(String key) {
+ return getEntry(key).getFlags();
+ }
+
+ /**
+ * Deletes the specified key in this table. The key can
+ * not be null.
+ *
+ * @param key the key name
+ */
+ public static void delete(String key) {
+ table.delete(key);
+ }
+
+ /**
+ * Put a boolean in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putBoolean(String key, boolean value) {
+ return getEntry(key).setBoolean(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultBoolean(String key, boolean defaultValue) {
+ return getEntry(key).setDefaultBoolean(defaultValue);
+ }
+
+ /**
+ * Returns the boolean the key maps to. If the key does not exist or is of
+ * different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static boolean getBoolean(String key, boolean defaultValue) {
+ return getEntry(key).getBoolean(defaultValue);
+ }
+
+ /**
+ * Put a number in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putNumber(String key, double value) {
+ return getEntry(key).setDouble(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultNumber(String key, double defaultValue) {
+ return getEntry(key).setDefaultDouble(defaultValue);
+ }
+
+ /**
+ * Returns the number the key maps to. If the key does not exist or is of
+ * different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static double getNumber(String key, double defaultValue) {
+ return getEntry(key).getDouble(defaultValue);
+ }
+
+ /**
+ * Put a string in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putString(String key, String value) {
+ return getEntry(key).setString(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultString(String key, String defaultValue) {
+ return getEntry(key).setDefaultString(defaultValue);
+ }
+
+ /**
+ * Returns the string the key maps to. If the key does not exist or is of
+ * different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static String getString(String key, String defaultValue) {
+ return getEntry(key).getString(defaultValue);
+ }
+
+ /**
+ * Put a boolean array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putBooleanArray(String key, boolean[] value) {
+ return getEntry(key).setBooleanArray(value);
+ }
+
+ /**
+ * Put a boolean array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putBooleanArray(String key, Boolean[] value) {
+ return getEntry(key).setBooleanArray(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultBooleanArray(String key, boolean[] defaultValue) {
+ return getEntry(key).setDefaultBooleanArray(defaultValue);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultBooleanArray(String key, Boolean[] defaultValue) {
+ return getEntry(key).setDefaultBooleanArray(defaultValue);
+ }
+
+ /**
+ * Returns the boolean array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static boolean[] getBooleanArray(String key, boolean[] defaultValue) {
+ return getEntry(key).getBooleanArray(defaultValue);
+ }
+
+ /**
+ * Returns the boolean array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static Boolean[] getBooleanArray(String key, Boolean[] defaultValue) {
+ return getEntry(key).getBooleanArray(defaultValue);
+ }
+
+ /**
+ * Put a number array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putNumberArray(String key, double[] value) {
+ return getEntry(key).setDoubleArray(value);
+ }
+
+ /**
+ * Put a number array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putNumberArray(String key, Double[] value) {
+ return getEntry(key).setNumberArray(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultNumberArray(String key, double[] defaultValue) {
+ return getEntry(key).setDefaultDoubleArray(defaultValue);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultNumberArray(String key, Double[] defaultValue) {
+ return getEntry(key).setDefaultNumberArray(defaultValue);
+ }
+
+ /**
+ * Returns the number array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static double[] getNumberArray(String key, double[] defaultValue) {
+ return getEntry(key).getDoubleArray(defaultValue);
+ }
+
+ /**
+ * Returns the number array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static Double[] getNumberArray(String key, Double[] defaultValue) {
+ return getEntry(key).getDoubleArray(defaultValue);
+ }
+
+ /**
+ * Put a string array in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putStringArray(String key, String[] value) {
+ return getEntry(key).setStringArray(value);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultStringArray(String key, String[] defaultValue) {
+ return getEntry(key).setDefaultStringArray(defaultValue);
+ }
+
+ /**
+ * Returns the string array the key maps to. If the key does not exist or is
+ * of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static String[] getStringArray(String key, String[] defaultValue) {
+ return getEntry(key).getStringArray(defaultValue);
+ }
+
+ /**
+ * Put a raw value (byte array) in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putRaw(String key, byte[] value) {
+ return getEntry(key).setRaw(value);
+ }
+
+ /**
+ * Put a raw value (bytes from a byte buffer) in the table.
+ * @param key the key to be assigned to
+ * @param value the value that will be assigned
+ * @param len the length of the value
+ * @return False if the table key already exists with a different type
+ */
+ public static boolean putRaw(String key, ByteBuffer value, int len) {
+ return getEntry(key).setRaw(value, len);
+ }
+
+ /**
+ * Gets the current value in the table, setting it if it does not exist.
+ * @param key the key
+ * @param defaultValue the default value to set if key does not exist.
+ * @return False if the table key exists with a different type
+ */
+ public static boolean setDefaultRaw(String key, byte[] defaultValue) {
+ return getEntry(key).setDefaultRaw(defaultValue);
+ }
+
+ /**
+ * Returns the raw value (byte array) the key maps to. If the key does not
+ * exist or is of different type, it will return the default value.
+ * @param key the key to look up
+ * @param defaultValue the value to be returned if no value is found
+ * @return the value associated with the given key or the given default value
+ * if there is no value associated with the key
+ */
+ public static byte[] getRaw(String key, byte[] defaultValue) {
+ return getEntry(key).getRaw(defaultValue);
+ }
+
+ /**
+ * Puts all sendable data to the dashboard.
+ */
+ public static synchronized void updateValues() {
+ for (Data data : tablesToData.values()) {
+ data.m_builder.updateTable();
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/SortedVector.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/SortedVector.java
new file mode 100644
index 0000000..f17d073
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/SortedVector.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.Vector;
+
+/**
+ * A vector that is sorted.
+ */
+public class SortedVector<E> extends Vector<E> {
+ /**
+ * Interface used to determine the order to place sorted objects.
+ */
+ public interface Comparator {
+ /**
+ * Compare the given two objects.
+ *
+ * <p>Should return -1, 0, or 1 if the first object is less than, equal to, or greater than the
+ * second, respectively.
+ *
+ * @param object1 First object to compare
+ * @param object2 Second object to compare
+ * @return -1, 0, or 1.
+ */
+ int compare(Object object1, Object object2);
+ }
+
+ private final Comparator m_comparator;
+
+ /**
+ * Create a new sorted vector and use the given comparator to determine order.
+ *
+ * @param comparator The comparator to use to determine what order to place the elements in this
+ * vector.
+ */
+ public SortedVector(Comparator comparator) {
+ m_comparator = comparator;
+ }
+
+ /**
+ * Adds an element in the Vector, sorted from greatest to least.
+ *
+ * @param element The element to add to the Vector
+ */
+ @Override
+ public synchronized void addElement(E element) {
+ int highBound = size();
+ int lowBound = 0;
+ while (highBound - lowBound > 0) {
+ int index = (highBound + lowBound) / 2;
+ int result = m_comparator.compare(element, elementAt(index));
+ if (result < 0) {
+ lowBound = index + 1;
+ } else if (result > 0) {
+ highBound = index;
+ } else {
+ lowBound = index;
+ highBound = index;
+ }
+ }
+ insertElementAt(element, lowBound);
+ }
+
+ /**
+ * Sort the vector.
+ */
+ @SuppressWarnings("unchecked")
+ public synchronized void sort() {
+ Object[] array = new Object[size()];
+ copyInto(array);
+ removeAllElements();
+ for (Object o : array) {
+ addElement((E) o);
+ }
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
new file mode 100644
index 0000000..2c0f8f2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.vision;
+
+import org.opencv.core.Mat;
+
+/**
+ * A vision pipeline is responsible for running a group of
+ * OpenCV algorithms to extract data from an image.
+ *
+ * @see VisionRunner
+ * @see VisionThread
+ *
+ * @deprecated Replaced with edu.wpi.first.vision.VisionPipeline
+ */
+@Deprecated
+public interface VisionPipeline {
+ /**
+ * Processes the image input and sets the result objects.
+ * Implementations should make these objects accessible.
+ */
+ void process(Mat image);
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
new file mode 100644
index 0000000..701b86e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.vision;
+
+import org.opencv.core.Mat;
+
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+
+/**
+ * A vision runner is a convenient wrapper object to make it easy to run vision pipelines
+ * from robot code. The easiest way to use this is to run it in a {@link VisionThread}
+ * and use the listener to take snapshots of the pipeline's outputs.
+ *
+ * @see VisionPipeline
+ * @see VisionThread
+ * @see <a href="package-summary.html">vision</a>
+ *
+ * @deprecated Replaced with edu.wpi.first.vision.VisionRunner
+ */
+@Deprecated
+public class VisionRunner<P extends VisionPipeline> {
+ private final CvSink m_cvSink = new CvSink("VisionRunner CvSink");
+ private final P m_pipeline;
+ private final Mat m_image = new Mat();
+ private final Listener<? super P> m_listener;
+ private volatile boolean m_enabled = true;
+
+ /**
+ * Listener interface for a callback that should run after a pipeline has processed its input.
+ *
+ * @param <P> the type of the pipeline this listener is for
+ */
+ @FunctionalInterface
+ public interface Listener<P extends VisionPipeline> {
+ /**
+ * Called when the pipeline has run. This shouldn't take much time to run because it will delay
+ * later calls to the pipeline's {@link VisionPipeline#process process} method. Copying the
+ * outputs and code that uses the copies should be <i>synchronized</i> on the same mutex to
+ * prevent multiple threads from reading and writing to the same memory at the same time.
+ *
+ * @param pipeline the vision pipeline that ran
+ */
+ void copyPipelineOutputs(P pipeline);
+
+ }
+
+ /**
+ * Creates a new vision runner. It will take images from the {@code videoSource}, send them to
+ * the {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert
+ * user code when it is safe to access the pipeline's outputs.
+ *
+ * @param videoSource the video source to use to supply images for the pipeline
+ * @param pipeline the vision pipeline to run
+ * @param listener a function to call after the pipeline has finished running
+ */
+ public VisionRunner(VideoSource videoSource, P pipeline, Listener<? super P> listener) {
+ this.m_pipeline = pipeline;
+ this.m_listener = listener;
+ m_cvSink.setSource(videoSource);
+ }
+
+ /**
+ * Runs the pipeline one time, giving it the next image from the video source specified
+ * in the constructor. This will block until the source either has an image or throws an error.
+ * If the source successfully supplied a frame, the pipeline's image input will be set,
+ * the pipeline will run, and the listener specified in the constructor will be called to notify
+ * it that the pipeline ran.
+ *
+ * <p>This method is exposed to allow teams to add additional functionality or have their own
+ * ways to run the pipeline. Most teams, however, should just use {@link #runForever} in its own
+ * thread using a {@link VisionThread}.</p>
+ */
+ public void runOnce() {
+ Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId();
+
+ if (id != null && Thread.currentThread().getId() == id.longValue()) {
+ throw new IllegalStateException(
+ "VisionRunner.runOnce() cannot be called from the main robot thread");
+ }
+ runOnceInternal();
+ }
+
+ private void runOnceInternal() {
+ long frameTime = m_cvSink.grabFrame(m_image);
+ if (frameTime == 0) {
+ // There was an error, report it
+ String error = m_cvSink.getError();
+ CameraServerSharedStore.getCameraServerShared().reportDriverStationError(error);
+ } else {
+ // No errors, process the image
+ m_pipeline.process(m_image);
+ m_listener.copyPipelineOutputs(m_pipeline);
+ }
+ }
+
+ /**
+ * A convenience method that calls {@link #runOnce()} in an infinite loop. This must
+ * be run in a dedicated thread, and cannot be used in the main robot thread because
+ * it will freeze the robot program.
+ *
+ * <p><strong>Do not call this method directly from the main thread.</strong></p>
+ *
+ * @throws IllegalStateException if this is called from the main robot thread
+ * @see VisionThread
+ */
+ public void runForever() {
+ Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId();
+
+ if (id != null && Thread.currentThread().getId() == id.longValue()) {
+ throw new IllegalStateException(
+ "VisionRunner.runForever() cannot be called from the main robot thread");
+ }
+ while (m_enabled && !Thread.interrupted()) {
+ runOnceInternal();
+ }
+ }
+
+ /**
+ * Stop a RunForever() loop.
+ */
+ public void stop() {
+ m_enabled = false;
+ }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
new file mode 100644
index 0000000..5184ba0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.vision;
+
+import edu.wpi.cscore.VideoSource;
+
+/**
+ * A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread;
+ * it does not prevent the program from exiting when all other non-daemon threads
+ * have finished running.
+ *
+ * @see VisionPipeline
+ * @see VisionRunner
+ * @see Thread#setDaemon(boolean)
+ *
+ * @deprecated Replaced with edu.wpi.first.vision.VisionThread
+ */
+@Deprecated
+public class VisionThread extends Thread {
+ /**
+ * Creates a vision thread that continuously runs a {@link VisionPipeline}.
+ *
+ * @param visionRunner the runner for a vision pipeline
+ */
+ public VisionThread(VisionRunner<?> visionRunner) {
+ super(visionRunner::runForever, "WPILib Vision Thread");
+ setDaemon(true);
+ }
+
+ /**
+ * Creates a new vision thread that continuously runs the given vision pipeline. This is
+ * equivalent to {@code new VisionThread(new VisionRunner<>(videoSource, pipeline, listener))}.
+ *
+ * @param videoSource the source for images the pipeline should process
+ * @param pipeline the pipeline to run
+ * @param listener the listener to copy outputs from the pipeline after it runs
+ * @param <P> the type of the pipeline
+ */
+ public <P extends VisionPipeline> VisionThread(VideoSource videoSource,
+ P pipeline,
+ VisionRunner.Listener<? super P> listener) {
+ this(new VisionRunner<>(videoSource, pipeline, listener));
+ }
+
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
new file mode 100644
index 0000000..01c8d73
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+/**
+ * Classes in the {@code edu.wpi.first.wpilibj.vision} package are designed to
+ * simplify using OpenCV vision processing code from a robot program.
+ *
+ * <p>An example use case for grabbing a yellow tote from 2015 in autonomous:
+ * <br>
+ * <pre><code>
+ * public class Robot extends IterativeRobot
+ * implements VisionRunner.Listener<MyFindTotePipeline> {
+ *
+ * // A USB camera connected to the roboRIO.
+ * private {@link edu.wpi.cscore.VideoSource VideoSource} usbCamera;
+ *
+ * // A vision pipeline. This could be handwritten or generated by GRIP.
+ * // This has to implement {@link edu.wpi.first.wpilibj.vision.VisionPipeline}.
+ * // For this example, assume that it's perfect and will always see the tote.
+ * private MyFindTotePipeline findTotePipeline;
+ * private {@link edu.wpi.first.wpilibj.vision.VisionThread} findToteThread;
+ *
+ * // The object to synchronize on to make sure the vision thread doesn't
+ * // write to variables the main thread is using.
+ * private final Object visionLock = new Object();
+ *
+ * // The pipeline outputs we want
+ * private boolean pipelineRan = false; // lets us know when the pipeline has actually run
+ * private double angleToTote = 0;
+ * private double distanceToTote = 0;
+ *
+ * {@literal @}Override
+ * public void {@link edu.wpi.first.wpilibj.vision.VisionRunner.Listener#copyPipelineOutputs
+ * copyPipelineOutputs(MyFindTotePipeline pipeline)} {
+ * synchronized (visionLock) {
+ * // Take a snapshot of the pipeline's output because
+ * // it may have changed the next time this method is called!
+ * this.pipelineRan = true;
+ * this.angleToTote = pipeline.getAngleToTote();
+ * this.distanceToTote = pipeline.getDistanceToTote();
+ * }
+ * }
+ *
+ * {@literal @}Override
+ * public void robotInit() {
+ * usbCamera = CameraServer.getInstance().startAutomaticCapture(0);
+ * findTotePipeline = new MyFindTotePipeline();
+ * findToteThread = new VisionThread(usbCamera, findTotePipeline, this);
+ * }
+ *
+ * {@literal @}Override
+ * public void autonomousInit() {
+ * findToteThread.start();
+ * }
+ *
+ * {@literal @}Override
+ * public void autonomousPeriodic() {
+ * double angle;
+ * double distance;
+ * synchronized (visionLock) {
+ * if (!pipelineRan) {
+ * // Wait until the pipeline has run
+ * return;
+ * }
+ * // Copy the outputs to make sure they're all from the same run
+ * angle = this.angleToTote;
+ * distance = this.distanceToTote;
+ * }
+ * if (!aimedAtTote()) {
+ * turnToAngle(angle);
+ * } else if (!droveToTote()) {
+ * driveDistance(distance);
+ * } else if (!grabbedTote()) {
+ * grabTote();
+ * } else {
+ * // Tote was grabbed and we're done!
+ * return;
+ * }
+ * }
+ *
+ * }
+ * </code></pre>
+ */
+@java.lang.Deprecated
+package edu.wpi.first.wpilibj.vision;
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java
new file mode 100644
index 0000000..608361a
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java
@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class CircularBufferTest {
+ private final double[] m_values = {751.848, 766.366, 342.657, 234.252, 716.126,
+ 132.344, 445.697, 22.727, 421.125, 799.913};
+ private final double[] m_addFirstOut = {799.913, 421.125, 22.727, 445.697, 132.344,
+ 716.126, 234.252, 342.657};
+ private final double[] m_addLastOut = {342.657, 234.252, 716.126, 132.344, 445.697,
+ 22.727, 421.125, 799.913};
+
+ @Test
+ void addFirstTest() {
+ CircularBuffer queue = new CircularBuffer(8);
+
+ for (double value : m_values) {
+ queue.addFirst(value);
+ }
+
+ for (int i = 0; i < m_addFirstOut.length; i++) {
+ assertEquals(m_addFirstOut[i], queue.get(i), 0.00005);
+ }
+ }
+
+ @Test
+ void addLastTest() {
+ CircularBuffer queue = new CircularBuffer(8);
+
+ for (double value : m_values) {
+ queue.addLast(value);
+ }
+
+ for (int i = 0; i < m_addLastOut.length; i++) {
+ assertEquals(m_addLastOut[i], queue.get(i), 0.00005);
+ }
+ }
+
+ @Test
+ void pushPopTest() {
+ CircularBuffer queue = new CircularBuffer(3);
+
+ // Insert three elements into the buffer
+ queue.addLast(1.0);
+ queue.addLast(2.0);
+ queue.addLast(3.0);
+
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+ assertEquals(3.0, queue.get(2), 0.00005);
+
+ /*
+ * The buffer is full now, so pushing subsequent elements will overwrite the
+ * front-most elements.
+ */
+
+ queue.addLast(4.0); // Overwrite 1 with 4
+
+ // The buffer now contains 2, 3, and 4
+ assertEquals(2.0, queue.get(0), 0.00005);
+ assertEquals(3.0, queue.get(1), 0.00005);
+ assertEquals(4.0, queue.get(2), 0.00005);
+
+ queue.addLast(5.0); // Overwrite 2 with 5
+
+ // The buffer now contains 3, 4, and 5
+ assertEquals(3.0, queue.get(0), 0.00005);
+ assertEquals(4.0, queue.get(1), 0.00005);
+ assertEquals(5.0, queue.get(2), 0.00005);
+
+ assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed
+
+ // The buffer now contains 3 and 4
+ assertEquals(3.0, queue.get(0), 0.00005);
+ assertEquals(4.0, queue.get(1), 0.00005);
+
+ assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed
+
+ // Leaving only one element with value == 4
+ assertEquals(4.0, queue.get(0), 0.00005);
+ }
+
+ @Test
+ void resetTest() {
+ CircularBuffer queue = new CircularBuffer(5);
+
+ for (int i = 0; i < 6; i++) {
+ queue.addLast(i);
+ }
+
+ queue.clear();
+
+ for (int i = 0; i < 5; i++) {
+ assertEquals(0.0, queue.get(i), 0.00005);
+ }
+ }
+
+ @Test
+ @SuppressWarnings("PMD.ExcessiveMethodLength")
+ void resizeTest() {
+ CircularBuffer queue = new CircularBuffer(5);
+
+ /* Buffer contains {1, 2, 3, _, _}
+ * ^ front
+ */
+ queue.addLast(1.0);
+ queue.addLast(2.0);
+ queue.addLast(3.0);
+
+ queue.resize(2);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.resize(5);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.clear();
+
+ /* Buffer contains {_, 1, 2, 3, _}
+ * ^ front
+ */
+ queue.addLast(0.0);
+ queue.addLast(1.0);
+ queue.addLast(2.0);
+ queue.addLast(3.0);
+ queue.removeFirst();
+
+ queue.resize(2);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.resize(5);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.clear();
+
+ /* Buffer contains {_, _, 1, 2, 3}
+ * ^ front
+ */
+ queue.addLast(0.0);
+ queue.addLast(0.0);
+ queue.addLast(1.0);
+ queue.addLast(2.0);
+ queue.addLast(3.0);
+ queue.removeFirst();
+ queue.removeFirst();
+
+ queue.resize(2);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.resize(5);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.clear();
+
+ /* Buffer contains {3, _, _, 1, 2}
+ * ^ front
+ */
+ queue.addLast(3.0);
+ queue.addFirst(2.0);
+ queue.addFirst(1.0);
+
+ queue.resize(2);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.resize(5);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.clear();
+
+ /* Buffer contains {2, 3, _, _, 1}
+ * ^ front
+ */
+ queue.addLast(2.0);
+ queue.addLast(3.0);
+ queue.addFirst(1.0);
+
+ queue.resize(2);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ queue.resize(5);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+
+ // Test addLast() after resize
+ queue.addLast(3.0);
+ assertEquals(1.0, queue.get(0), 0.00005);
+ assertEquals(2.0, queue.get(1), 0.00005);
+ assertEquals(3.0, queue.get(2), 0.00005);
+
+ // Test addFirst() after resize
+ queue.addFirst(4.0);
+ assertEquals(4.0, queue.get(0), 0.00005);
+ assertEquals(1.0, queue.get(1), 0.00005);
+ assertEquals(2.0, queue.get(2), 0.00005);
+ assertEquals(3.0, queue.get(3), 0.00005);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
new file mode 100644
index 0000000..6990b50
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.extension.BeforeAllCallback;
+import org.junit.jupiter.api.extension.ExtensionContext;
+import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.DriverStationSim;
+
+public final class MockHardwareExtension implements BeforeAllCallback {
+ private static ExtensionContext getRoot(ExtensionContext context) {
+ return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
+ }
+
+ @Override
+ public void beforeAll(ExtensionContext context) throws Exception {
+ getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+ initializeHardware();
+ return true;
+ }, Boolean.class);
+ }
+
+ private void initializeHardware() {
+ HAL.initialize(500, 0);
+ DriverStationSim dsSim = new DriverStationSim();
+ dsSim.setDsAttached(true);
+ dsSim.setAutonomous(false);
+ dsSim.setEnabled(true);
+ dsSim.setTest(true);
+
+
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java
new file mode 100644
index 0000000..d183723
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+public class MockSpeedController implements SpeedController {
+ private double m_speed;
+ private boolean m_isInverted;
+
+ @Override
+ public void set(double speed) {
+ m_speed = m_isInverted ? -speed : speed;
+ }
+
+ @Override
+ public double get() {
+ return m_speed;
+ }
+
+ @Override
+ public void setInverted(boolean isInverted) {
+ m_isInverted = isInverted;
+ }
+
+ @Override
+ public boolean getInverted() {
+ return m_isInverted;
+ }
+
+ @Override
+ public void disable() {
+ m_speed = 0;
+ }
+
+ @Override
+ public void stopMotor() {
+ disable();
+ }
+
+ @Override
+ public void pidWrite(double output) {
+ set(output);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
new file mode 100644
index 0000000..cbf3f99
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PIDToleranceTest {
+ private PIDController m_pid;
+ private static final double m_setPoint = 50.0;
+ private static final double m_tolerance = 10.0;
+ private static final double m_range = 200;
+
+ private class FakeInput implements PIDSource {
+ public double m_val;
+
+ FakeInput() {
+ m_val = 0;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return PIDSourceType.kDisplacement;
+ }
+
+ @Override
+ public double pidGet() {
+ return m_val;
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType arg0) {
+ }
+ }
+
+ private FakeInput m_inp;
+ private final PIDOutput m_out = new PIDOutput() {
+ @Override
+ public void pidWrite(double out) {
+ }
+
+ };
+
+
+ @BeforeEach
+ void setUp() {
+ m_inp = new FakeInput();
+ m_pid = new PIDController(0.05, 0.0, 0.0, m_inp, m_out);
+ m_pid.setInputRange(-m_range / 2, m_range / 2);
+ }
+
+ @AfterEach
+ void tearDown() {
+ m_pid.close();
+ m_pid = null;
+ }
+
+ @Test
+ void absoluteToleranceTest() {
+ m_pid.setAbsoluteTolerance(m_tolerance);
+ m_pid.setSetpoint(m_setPoint);
+ m_pid.enable();
+ Timer.delay(1);
+ assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
+ + m_pid.getError());
+ m_inp.m_val = m_setPoint + m_tolerance / 2;
+ Timer.delay(1.0);
+ assertTrue(m_pid.onTarget(), "Error was not in tolerance when it should have been. Error was "
+ + m_pid.getError());
+ m_inp.m_val = m_setPoint + 10 * m_tolerance;
+ Timer.delay(1.0);
+ assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
+ + m_pid.getError());
+ }
+
+ @Test
+ void percentToleranceTest() {
+ m_pid.setPercentTolerance(m_tolerance);
+ m_pid.setSetpoint(m_setPoint);
+ m_pid.enable();
+ assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
+ + m_pid.getError());
+ //half of percent tolerance away from setPoint
+ m_inp.m_val = m_setPoint + m_tolerance / 200 * m_range;
+ Timer.delay(1.0);
+ assertTrue(m_pid.onTarget(), "Error was not in tolerance when it should have been. Error was "
+ + m_pid.getError());
+ //double percent tolerance away from setPoint
+ m_inp.m_val = m_setPoint + m_tolerance / 50 * m_range;
+ Timer.delay(1.0);
+ assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
+ + m_pid.getError());
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
new file mode 100644
index 0000000..576eb8b
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+class RobotControllerTest extends UtilityClassTest {
+ RobotControllerTest() {
+ super(RobotController.class);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java
new file mode 100644
index 0000000..62e2f46
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Arrays;
+import java.util.stream.DoubleStream;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class SpeedControllerGroupTest {
+ private static Stream<Arguments> speedControllerArguments() {
+ return IntStream.of(1, 2, 3).mapToObj(number -> {
+ SpeedController[] speedControllers = Stream.generate(MockSpeedController::new)
+ .limit(number)
+ .toArray(SpeedController[]::new);
+ SpeedControllerGroup group = new SpeedControllerGroup(speedControllers[0],
+ Arrays.copyOfRange(speedControllers, 1, speedControllers.length));
+ return Arguments.of(group, speedControllers);
+ });
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void setTest(final SpeedControllerGroup group, final SpeedController[] speedControllers) {
+ group.set(1.0);
+
+ assertArrayEquals(DoubleStream.generate(() -> 1.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void getInvertedTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.setInverted(true);
+
+ assertTrue(group.getInverted());
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void setInvertedDoesNotModifySpeedControllersTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.setInverted(true);
+
+ assertArrayEquals(Stream.generate(() -> false).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).map(SpeedController::getInverted).toArray());
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void setInvertedDoesInvertTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.setInverted(true);
+ group.set(1.0);
+
+ assertArrayEquals(DoubleStream.generate(() -> -1.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void disableTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.set(1.0);
+ group.disable();
+
+ assertArrayEquals(DoubleStream.generate(() -> 0.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void stopMotorTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.set(1.0);
+ group.stopMotor();
+
+ assertArrayEquals(DoubleStream.generate(() -> 0.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+
+ @ParameterizedTest
+ @MethodSource("speedControllerArguments")
+ void pidWriteTest(final SpeedControllerGroup group,
+ final SpeedController[] speedControllers) {
+ group.pidWrite(1.0);
+
+ assertArrayEquals(DoubleStream.generate(() -> 1.0).limit(speedControllers.length).toArray(),
+ Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
+ 0.00005);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
new file mode 100644
index 0000000..1265429
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.DynamicTest.dynamicTest;
+
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class UtilityClassTest {
+ private final Class m_clazz;
+
+ protected UtilityClassTest(Class clazz) {
+ m_clazz = clazz;
+ }
+
+ @Test
+ public void singleConstructorTest() {
+ assertEquals(1, m_clazz.getDeclaredConstructors().length,
+ "More than one constructor defined");
+ }
+
+ @Test
+ public void constructorPrivateTest() {
+ Constructor constructor = m_clazz.getDeclaredConstructors()[0];
+
+ assertFalse(constructor.isAccessible(), "Constructor is not private");
+ }
+
+ @Test
+ public void constructorReflectionTest() {
+ Constructor constructor = m_clazz.getDeclaredConstructors()[0];
+ constructor.setAccessible(true);
+ assertThrows(InvocationTargetException.class, () -> constructor.newInstance());
+ }
+
+ @TestFactory
+ Stream<DynamicTest> publicMethodsStaticTestFactory() {
+ return Arrays.stream(m_clazz.getDeclaredMethods())
+ .filter(method -> Modifier.isPublic(method.getModifiers()))
+ .map(method -> dynamicTest(method.getName(),
+ () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
new file mode 100644
index 0000000..9ed5db8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
@@ -0,0 +1,217 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class WatchdogTest {
+ @Test
+ void enableDisableTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(0.4, () -> {
+ watchdogCounter.addAndGet(1);
+ });
+
+ System.out.println("Run 1");
+ watchdog.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ System.out.println("Run 2");
+ watchdogCounter.set(0);
+ watchdog.enable();
+ try {
+ Thread.sleep(600);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(1, watchdogCounter.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+
+ // Run 3
+ watchdogCounter.set(0);
+ watchdog.enable();
+ try {
+ Thread.sleep(1000);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(1, watchdogCounter.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+ }
+
+ @Test
+ void resetTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(0.4, () -> {
+ watchdogCounter.addAndGet(1);
+ });
+
+ watchdog.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.reset();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+ }
+
+ @Test
+ void setTimeoutTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(1.0, () -> {
+ watchdogCounter.addAndGet(1);
+ });
+
+ watchdog.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.setTimeout(0.2);
+
+ assertEquals(0.2, watchdog.getTimeout());
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ try {
+ Thread.sleep(300);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.disable();
+
+ assertEquals(1, watchdogCounter.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+ }
+
+ @Test
+ void isExpiredTest() {
+ final Watchdog watchdog = new Watchdog(0.2, () -> {
+ });
+ watchdog.enable();
+
+ assertFalse(watchdog.isExpired());
+ try {
+ Thread.sleep(300);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ assertTrue(watchdog.isExpired());
+ }
+
+ @Test
+ void epochsTest() {
+ final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+ final Watchdog watchdog = new Watchdog(0.4, () -> {
+ watchdogCounter.addAndGet(1);
+ });
+
+ System.out.println("Run 1");
+ watchdog.enable();
+ watchdog.addEpoch("Epoch 1");
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.addEpoch("Epoch 2");
+ try {
+ Thread.sleep(100);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.addEpoch("Epoch 3");
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+ System.out.println("Run 2");
+ watchdog.enable();
+ watchdog.addEpoch("Epoch 1");
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.reset();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog.addEpoch("Epoch 2");
+ watchdog.disable();
+
+ assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+ }
+
+ @Test
+ void multiWatchdogTest() {
+ final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
+ final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
+
+ final Watchdog watchdog1 = new Watchdog(0.2, () -> {
+ watchdogCounter1.addAndGet(1);
+ });
+ final Watchdog watchdog2 = new Watchdog(0.6, () -> {
+ watchdogCounter2.addAndGet(1);
+ });
+
+ watchdog2.enable();
+ try {
+ Thread.sleep(200);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
+ assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
+
+ // Sleep enough such that only the watchdog enabled later times out first
+ watchdog1.enable();
+ try {
+ Thread.sleep(300);
+ } catch (InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ watchdog1.disable();
+ watchdog2.disable();
+
+ assertEquals(1, watchdogCounter1.get(),
+ "Watchdog either didn't trigger or triggered more than once");
+ assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
new file mode 100644
index 0000000..e804d0c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.can;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.can.CANJNI;
+import edu.wpi.first.hal.can.CANStatus;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+class CANStatusTest {
+ @Test
+ void canStatusGetDoesntThrow() {
+ HAL.initialize(500, 0);
+ CANStatus status = new CANStatus();
+ assertDoesNotThrow(() -> CANJNI.GetCANStatus(status));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
new file mode 100644
index 0000000..1fa9aa8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.BeforeEach;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * The basic test for all {@link Command} tests.
+ */
+public abstract class AbstractCommandTest {
+ @BeforeEach
+ void commandSetup() {
+ Scheduler.getInstance().removeAll();
+ Scheduler.getInstance().enable();
+ }
+
+ public class ASubsystem extends Subsystem {
+ Command m_command;
+
+ @Override
+ protected void initDefaultCommand() {
+ if (m_command != null) {
+ setDefaultCommand(m_command);
+ }
+ }
+
+ public void init(Command command) {
+ m_command = command;
+ }
+ }
+
+
+ protected void assertCommandState(MockCommand command, int initialize, int execute,
+ int isFinished, int end, int interrupted) {
+ assertAll(
+ () -> assertEquals(initialize, command.getInitializeCount()),
+ () -> assertEquals(execute, command.getExecuteCount()),
+ () -> assertEquals(isFinished, command.getIsFinishedCount()),
+ () -> assertEquals(end, command.getEndCount()),
+ () -> assertEquals(interrupted, command.getInterruptedCount())
+ );
+ }
+
+ protected void sleep(int time) {
+ assertDoesNotThrow(() -> Thread.sleep(time));
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
new file mode 100644
index 0000000..3686c78
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.buttons.InternalButton;
+
+
+/**
+ * Test that covers the {@link edu.wpi.first.wpilibj.buttons.Button} with the {@link Command}
+ * library.
+ */
+class ButtonTest extends AbstractCommandTest {
+ private InternalButton m_button1;
+ private InternalButton m_button2;
+
+ @BeforeEach
+ void setUp() {
+ m_button1 = new InternalButton();
+ m_button2 = new InternalButton();
+ }
+
+ /**
+ * Simple Button Test.
+ */
+ @Test
+ void buttonTest() {
+ final MockCommand command1 = new MockCommand();
+ final MockCommand command2 = new MockCommand();
+ final MockCommand command3 = new MockCommand();
+ final MockCommand command4 = new MockCommand();
+
+ m_button1.whenPressed(command1);
+ m_button1.whenReleased(command2);
+ m_button1.whileHeld(command3);
+ m_button2.whileHeld(command4);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ m_button1.setPressed(true);
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 1, 1, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 2, 2, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ m_button2.setPressed(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 3, 3, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 0);
+ assertCommandState(command4, 1, 1, 1, 0, 0);
+ m_button1.setPressed(false);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 5, 5, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 6, 6, 0, 0);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 0);
+ m_button2.setPressed(false);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 0);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ command1.cancel();
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 1);
+ assertCommandState(command2, 1, 3, 3, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ command2.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 1);
+ assertCommandState(command2, 1, 4, 4, 1, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 1);
+ assertCommandState(command2, 1, 4, 4, 1, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
new file mode 100644
index 0000000..b3b7ad9
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandParallelGroupTest extends AbstractCommandTest {
+ /**
+ * Simple Parallel Command Group With 2 commands one command terminates first.
+ */
+ @Test
+ void parallelCommandGroupWithTwoCommandsTest() {
+ final MockCommand command1 = new MockCommand();
+ final MockCommand command2 = new MockCommand();
+
+ final CommandGroup commandGroup = new CommandGroup();
+ commandGroup.addParallel(command1);
+ commandGroup.addParallel(command2);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ commandGroup.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ command1.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 1, 0);
+ assertCommandState(command2, 1, 3, 3, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 1, 0);
+ assertCommandState(command2, 1, 4, 4, 0, 0);
+ command2.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 1, 0);
+ assertCommandState(command2, 1, 5, 5, 1, 0);
+
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
new file mode 100644
index 0000000..feb80bc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandScheduleTest extends AbstractCommandTest {
+ /**
+ * Simple scheduling of a command and making sure the command is run and successfully terminates.
+ */
+ @Test
+ void runAndTerminateTest() {
+ final MockCommand command = new MockCommand();
+ command.start();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ command.setHasFinished(true);
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 1, 0);
+ }
+
+ /**
+ * Simple scheduling of a command and making sure the command is run and cancels correctly.
+ */
+ @Test
+ void runAndCancelTest() {
+ final MockCommand command = new MockCommand();
+ command.start();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 0);
+ command.cancel();
+ assertCommandState(command, 1, 3, 3, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 1);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
new file mode 100644
index 0000000..5667b05
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.logging.Logger;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandSequentialGroupTest extends AbstractCommandTest {
+ private static final Logger logger = Logger.getLogger(CommandSequentialGroupTest.class.getName());
+
+ /**
+ * Simple Command Group With 3 commands that all depend on a subsystem. Some commands have a
+ * timeout.
+ */
+ @Test
+ public void testThreeCommandOnSubSystem() {
+ logger.fine("Begining Test");
+ final ASubsystem subsystem = new ASubsystem();
+
+ logger.finest("Creating Mock Command1");
+ final MockCommand command1 = new MockCommand(subsystem);
+ logger.finest("Creating Mock Command2");
+ final MockCommand command2 = new MockCommand(subsystem);
+ logger.finest("Creating Mock Command3");
+ final MockCommand command3 = new MockCommand(subsystem);
+
+ logger.finest("Creating Command Group");
+ final CommandGroup commandGroup = new CommandGroup();
+ commandGroup.addSequential(command1, 1.0);
+ commandGroup.addSequential(command2, 2.0);
+ commandGroup.addSequential(command3);
+
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ logger.finest("Starting Command group");
+ commandGroup.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ sleep(1250); // command 1 timeout
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ sleep(2500); // command 2 timeout
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 1, 1, 0, 0);
+
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 2, 2, 0, 0);
+ command3.setHasFinished(true);
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 3, 3, 1, 0);
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
new file mode 100644
index 0000000..4ba965f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandSupersedeTest extends AbstractCommandTest {
+ /**
+ * Testing one command superseding another because of dependencies.
+ */
+ @Test
+ void oneCommandSupersedingAnotherBecauseOfDependenciesTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+ final MockCommand command1 = new MockCommand(subsystem);
+
+ final MockCommand command2 = new MockCommand(subsystem);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command1.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command2.start();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 1, 3, 3, 0, 0);
+ }
+
+ /**
+ * Testing one command failing superseding another because of dependencies because the first
+ * command cannot be interrupted.
+ */
+ @Test
+ @SuppressWarnings("PMD.NonStaticInitializer")
+ void commandFailingSupersedingBecauseFirstCanNotBeInterruptedTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+ final MockCommand command1 = new MockCommand(subsystem) {
+ {
+ setInterruptible(false);
+ }
+ };
+
+ final MockCommand command2 = new MockCommand(subsystem);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command1.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command2.start();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
new file mode 100644
index 0000000..ff7ad86
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Test a {@link Command} that times out.
+ */
+class CommandTimeoutTest extends AbstractCommandTest {
+ /**
+ * Command 2 second Timeout Test.
+ */
+ @Test
+ void twoSecondTimeoutTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+
+ final MockCommand command = new MockCommand(subsystem, 2) {
+ @Override
+ public boolean isFinished() {
+ return super.isFinished() || isTimedOut();
+ }
+ };
+
+ command.start();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 0);
+ sleep(2500);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 4, 4, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 4, 4, 1, 0);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
new file mode 100644
index 0000000..5f3285c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
@@ -0,0 +1,345 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertSame;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class ConditionalCommandTest extends AbstractCommandTest {
+ MockConditionalCommand m_command;
+ MockConditionalCommand m_commandNull;
+ MockCommand m_onTrue;
+ MockCommand m_onFalse;
+ MockSubsystem m_subsys;
+ Boolean m_condition;
+
+ @BeforeEach
+ void initCommands() {
+ m_subsys = new MockSubsystem();
+ m_onTrue = new MockCommand(m_subsys);
+ m_onFalse = new MockCommand(m_subsys);
+ m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+ m_commandNull = new MockConditionalCommand(m_onTrue, null);
+ }
+
+ protected void assertConditionalCommandState(MockConditionalCommand command, int initialize,
+ int execute, int isFinished, int end,
+ int interrupted) {
+ assertEquals(initialize, command.getInitializeCount());
+ assertEquals(execute, command.getExecuteCount());
+ assertEquals(isFinished, command.getIsFinishedCount());
+ assertEquals(end, command.getEndCount());
+ assertEquals(interrupted, command.getInterruptedCount());
+ }
+
+ @Test
+ void onTrueTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+ assertTrue(m_onTrue.getInitializeCount() > 0, "Did not initialize the true command");
+ assertSame(m_onFalse.getInitializeCount(), 0, "Initialized the false command");
+ }
+
+ @Test
+ void onFalseTest() {
+ m_command.setCondition(false);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onFalse
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onFalse
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 1, 1, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 2, 2, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onFalse.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+ assertTrue(m_onFalse.getInitializeCount() > 0, "Did not initialize the false command");
+ assertSame(m_onTrue.getInitializeCount(), 0, "Initialized the true command");
+ }
+
+ @Test
+ void cancelSubCommandTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.cancel();
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+ }
+
+ @Test
+ void cancelRequiresTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onFalse.start();
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 0, 1);
+ assertCommandState(m_onFalse, 1, 1, 1, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 1);
+ }
+
+ @Test
+ void cancelCondCommandTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_command.cancel();
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 1);
+ }
+
+ @Test
+ void onTrueTwiceTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+ m_onTrue.resetCounters();
+ m_command.resetCounters();
+ m_command.setCondition(true);
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+ }
+
+ @Test
+ void onTrueInstantTest() {
+ m_command.setCondition(true);
+ m_onTrue.setHasFinished(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+ }
+
+ @Test
+ void onFalseNullTest() {
+ m_commandNull.setCondition(false);
+
+ Scheduler.getInstance().add(m_commandNull);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onFalse
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onFalse
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
new file mode 100644
index 0000000..1cfec7f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Tests the {@link Command} library.
+ */
+public class DefaultCommandTest extends AbstractCommandTest {
+ /**
+ * Testing of default commands where the interrupting command ends itself.
+ */
+ @Test
+ void defaultCommandWhereTheInteruptingCommandEndsItselfTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+
+ final MockCommand defaultCommand = new MockCommand(subsystem);
+
+ final MockCommand anotherCommand = new MockCommand(subsystem);
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ subsystem.init(defaultCommand);
+
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+
+ anotherCommand.start();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ anotherCommand.setHasFinished(true);
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 4, 4, 0, 1);
+ assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 5, 5, 0, 1);
+ assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+ }
+
+
+ /**
+ * Testing of default commands where the interrupting command is canceled.
+ */
+ @Test
+ void defaultCommandsInterruptingCommandCanceledTest() {
+ final ASubsystem subsystem = new ASubsystem();
+ final MockCommand defaultCommand = new MockCommand(subsystem);
+ final MockCommand anotherCommand = new MockCommand(subsystem);
+
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ subsystem.init(defaultCommand);
+ subsystem.initDefaultCommand();
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+
+ anotherCommand.start();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ anotherCommand.cancel();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 4, 4, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 5, 5, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
new file mode 100644
index 0000000..bee2f80
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A class to simulate a simple command. The command keeps track of how many times each method was
+ * called.
+ */
+public class MockCommand extends Command {
+ private int m_initializeCount;
+ private int m_executeCount;
+ private int m_isFinishedCount;
+ private boolean m_hasFinished;
+ private int m_endCount;
+ private int m_interruptedCount;
+
+ public MockCommand(Subsystem subsys) {
+ super();
+ requires(subsys);
+ }
+
+ public MockCommand(Subsystem subsys, double timeout) {
+ this(subsys);
+ setTimeout(timeout);
+ }
+
+ public MockCommand() {
+ super();
+ }
+
+ @Override
+ protected void initialize() {
+ ++m_initializeCount;
+ }
+
+ @Override
+ protected void execute() {
+ ++m_executeCount;
+ }
+
+ @Override
+ protected boolean isFinished() {
+ ++m_isFinishedCount;
+ return isHasFinished();
+ }
+
+ @Override
+ protected void end() {
+ ++m_endCount;
+ }
+
+ @Override
+ protected void interrupted() {
+ ++m_interruptedCount;
+ }
+
+
+ /**
+ * How many times the initialize method has been called.
+ */
+ public int getInitializeCount() {
+ return m_initializeCount;
+ }
+
+ /**
+ * If the initialize method has been called at least once.
+ */
+ public boolean hasInitialized() {
+ return getInitializeCount() > 0;
+ }
+
+ /**
+ * How many time the execute method has been called.
+ */
+ public int getExecuteCount() {
+ return m_executeCount;
+ }
+
+ /**
+ * How many times the isFinished method has been called.
+ */
+ public int getIsFinishedCount() {
+ return m_isFinishedCount;
+ }
+
+ /**
+ * Get what value the isFinished method will return.
+ *
+ * @return what value the isFinished method will return.
+ */
+ public boolean isHasFinished() {
+ return m_hasFinished;
+ }
+
+ /**
+ * Set what value the isFinished method will return.
+ *
+ * @param hasFinished set what value the isFinished method will return.
+ */
+ public void setHasFinished(boolean hasFinished) {
+ m_hasFinished = hasFinished;
+ }
+
+ /**
+ * How many times the end method has been called.
+ */
+ public int getEndCount() {
+ return m_endCount;
+ }
+
+ /**
+ * If the end method has been called at least once.
+ */
+ public boolean hasEnd() {
+ return getEndCount() > 0;
+ }
+
+ /**
+ * How many times the interrupted method has been called.
+ */
+ public int getInterruptedCount() {
+ return m_interruptedCount;
+ }
+
+ /**
+ * If the interrupted method has been called at least once.
+ */
+ public boolean hasInterrupted() {
+ return getInterruptedCount() > 0;
+ }
+
+ /**
+ * Reset internal counters.
+ */
+ public void resetCounters() {
+ m_initializeCount = 0;
+ m_executeCount = 0;
+ m_isFinishedCount = 0;
+ m_hasFinished = false;
+ m_endCount = 0;
+ m_interruptedCount = 0;
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
new file mode 100644
index 0000000..876f1c5
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+public class MockConditionalCommand extends ConditionalCommand {
+ private boolean m_condition;
+ private int m_initializeCount;
+ private int m_executeCount;
+ private int m_isFinishedCount;
+ private int m_endCount;
+ private int m_interruptedCount;
+
+ public MockConditionalCommand(MockCommand onTrue, MockCommand onFalse) {
+ super(onTrue, onFalse);
+ }
+
+ @Override
+ protected boolean condition() {
+ return m_condition;
+ }
+
+ public void setCondition(boolean condition) {
+ this.m_condition = condition;
+ }
+
+ @Override
+ protected void initialize() {
+ ++m_initializeCount;
+ }
+
+ @Override
+ protected void execute() {
+ ++m_executeCount;
+ }
+
+ @Override
+ protected boolean isFinished() {
+ ++m_isFinishedCount;
+ return super.isFinished();
+ }
+
+ @Override
+ protected void end() {
+ ++m_endCount;
+ }
+
+ @Override
+ protected void interrupted() {
+ ++m_interruptedCount;
+ }
+
+
+ /**
+ * How many times the initialize method has been called.
+ */
+ public int getInitializeCount() {
+ return m_initializeCount;
+ }
+
+ /**
+ * If the initialize method has been called at least once.
+ */
+ public boolean hasInitialized() {
+ return getInitializeCount() > 0;
+ }
+
+ /**
+ * How many time the execute method has been called.
+ */
+ public int getExecuteCount() {
+ return m_executeCount;
+ }
+
+ /**
+ * How many times the isFinished method has been called.
+ */
+ public int getIsFinishedCount() {
+ return m_isFinishedCount;
+ }
+
+ /**
+ * How many times the end method has been called.
+ */
+ public int getEndCount() {
+ return m_endCount;
+ }
+
+ /**
+ * If the end method has been called at least once.
+ */
+ public boolean hasEnd() {
+ return getEndCount() > 0;
+ }
+
+ /**
+ * How many times the interrupted method has been called.
+ */
+ public int getInterruptedCount() {
+ return m_interruptedCount;
+ }
+
+ /**
+ * If the interrupted method has been called at least once.
+ */
+ public boolean hasInterrupted() {
+ return getInterruptedCount() > 0;
+ }
+
+ /**
+ * Reset internal counters.
+ */
+ public void resetCounters() {
+ m_condition = false;
+ m_initializeCount = 0;
+ m_executeCount = 0;
+ m_isFinishedCount = 0;
+ m_endCount = 0;
+ m_interruptedCount = 0;
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
new file mode 100644
index 0000000..df39cd1
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A class to simulate a simple subsystem.
+ */
+public class MockSubsystem extends Subsystem {
+ @Override
+ protected void initDefaultCommand() {}
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
new file mode 100644
index 0000000..f70f3f8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.hal;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+
+class JNITest {
+ @Test
+ void jniNtcoreLinkTest() {
+ // Test to verify that the JNI test link works correctly.
+ NetworkTablesJNI.flush(NetworkTablesJNI.getDefaultInstance());
+ }
+
+ @Test
+ void jniHalLinkTest() {
+ HAL.initialize(500, 0);
+ // Test to verify that the JNI test link works correctly.
+ HALUtil.getHALRuntimeType();
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
new file mode 100644
index 0000000..98b8489
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.hal;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.hal.sim.mockdata.DriverStationDataJNI;
+import edu.wpi.first.wpilibj.DriverStation.MatchType;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MatchInfoDataTest {
+ @Test
+ void testSetMatchInfo() {
+
+ MatchType matchType = MatchType.Qualification;
+ DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal());
+
+ MatchInfoData outMatchInfo = new MatchInfoData();
+ HAL.getMatchInfo(outMatchInfo);
+
+ assertAll(
+ () -> assertEquals("Event Name", outMatchInfo.eventName),
+ () -> assertEquals(matchType.ordinal(), outMatchInfo.matchType),
+ () -> assertEquals(174, outMatchInfo.matchNumber),
+ () -> assertEquals(191, outMatchInfo.replayNumber),
+ () -> assertEquals("Game Message", outMatchInfo.gameSpecificMessage)
+ );
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
new file mode 100644
index 0000000..da1757c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.livewindow;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+class LiveWindowTest extends UtilityClassTest {
+ LiveWindowTest() {
+ super(LiveWindow.class);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
new file mode 100644
index 0000000..6cbeae2
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * A mock sendable that marks itself as an actuator.
+ */
+public class MockActuatorSendable extends SendableBase {
+ public MockActuatorSendable(String name) {
+ super(false);
+ setName(name);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setActuator(true);
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
new file mode 100644
index 0000000..9824f11
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class ShuffleboardInstanceTest {
+ private NetworkTableInstance m_ntInstance;
+ private ShuffleboardInstance m_shuffleboardInstance;
+
+ @BeforeEach
+ void setupInstance() {
+ m_ntInstance = NetworkTableInstance.create();
+ m_shuffleboardInstance = new ShuffleboardInstance(m_ntInstance);
+ }
+
+ @AfterEach
+ void tearDownInstance() {
+ m_ntInstance.close();
+ }
+
+ @Test
+ void testPathFluent() {
+ NetworkTableEntry entry = m_shuffleboardInstance.getTab("Tab Title")
+ .getLayout("Layout Title", "List Layout")
+ .add("Data", "string")
+ .withWidget("Text View")
+ .getEntry();
+
+ assertAll(
+ () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
+ () -> assertEquals("/Shuffleboard/Tab Title/Layout Title/Data", entry.getName(),
+ "Entry path generated incorrectly"));
+ }
+
+ @Test
+ void testNestedLayoutsFluent() {
+ NetworkTableEntry entry = m_shuffleboardInstance.getTab("Tab")
+ .getLayout("First", "List")
+ .getLayout("Second", "List")
+ .getLayout("Third", "List")
+ .getLayout("Fourth", "List")
+ .add("Value", "string")
+ .getEntry();
+
+ assertAll(
+ () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
+ () -> assertEquals("/Shuffleboard/Tab/First/Second/Third/Fourth/Value", entry.getName(),
+ "Entry path generated incorrectly"));
+ }
+
+ @Test
+ void testNestedLayoutsOop() {
+ ShuffleboardTab tab = m_shuffleboardInstance.getTab("Tab");
+ ShuffleboardLayout first = tab.getLayout("First", "List");
+ ShuffleboardLayout second = first.getLayout("Second", "List");
+ ShuffleboardLayout third = second.getLayout("Third", "List");
+ ShuffleboardLayout fourth = third.getLayout("Fourth", "List");
+ SimpleWidget widget = fourth.add("Value", "string");
+ NetworkTableEntry entry = widget.getEntry();
+
+ assertAll(
+ () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
+ () -> assertEquals("/Shuffleboard/Tab/First/Second/Third/Fourth/Value", entry.getName(),
+ "Entry path generated incorrectly"));
+ }
+
+ @Test
+ void testLayoutTypeIsSet() {
+ String layoutType = "Type";
+ m_shuffleboardInstance.getTab("Tab")
+ .getLayout("Title", layoutType);
+ m_shuffleboardInstance.update();
+ NetworkTableEntry entry = m_ntInstance.getEntry(
+ "/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
+ assertEquals(layoutType, entry.getString("Not Set"), "Layout type not set");
+ }
+
+ @Test
+ void testNestedActuatorWidgetsAreDisabled() {
+ m_shuffleboardInstance.getTab("Tab")
+ .getLayout("Title", "Layout")
+ .add(new MockActuatorSendable("Actuator"));
+ NetworkTableEntry controllableEntry =
+ m_ntInstance.getEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
+
+ m_shuffleboardInstance.update();
+
+ // Note: we use the unsafe `getBoolean()` method because if the value is NOT a boolean, or if it
+ // is not present, then something has clearly gone very, very wrong
+ boolean controllable = controllableEntry.getValue().getBoolean();
+
+ // Sanity check
+ assertTrue(controllable, "The nested actuator widget should be enabled by default");
+ m_shuffleboardInstance.disableActuatorWidgets();
+ controllable = controllableEntry.getValue().getBoolean();
+ assertFalse(controllable, "The nested actuator widget should have been disabled");
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
new file mode 100644
index 0000000..fe48c13
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+@SuppressWarnings({"PMD.TooManyMethods"})
+public class ShuffleboardTabTest {
+ private NetworkTableInstance m_ntInstance;
+ private ShuffleboardTab m_tab;
+ private ShuffleboardInstance m_instance;
+
+ @BeforeEach
+ void setup() {
+ m_ntInstance = NetworkTableInstance.create();
+ m_instance = new ShuffleboardInstance(m_ntInstance);
+ m_tab = m_instance.getTab("Tab");
+ }
+
+ @AfterEach
+ void tearDown() {
+ m_ntInstance.close();
+ }
+
+ @Test
+ void testAddDouble() {
+ NetworkTableEntry entry = m_tab.add("Double", 1.0).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Double", entry.getName()),
+ () -> assertEquals(1.0, entry.getValue().getDouble()));
+ }
+
+ @Test
+ void testAddInteger() {
+ NetworkTableEntry entry = m_tab.add("Int", 1).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Int", entry.getName()),
+ () -> assertEquals(1.0, entry.getValue().getDouble()));
+ }
+
+ @Test
+ void testAddLong() {
+ NetworkTableEntry entry = m_tab.add("Long", 1L).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Long", entry.getName()),
+ () -> assertEquals(1.0, entry.getValue().getDouble()));
+ }
+
+
+ @Test
+ void testAddBoolean() {
+ NetworkTableEntry entry = m_tab.add("Bool", false).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Bool", entry.getName()),
+ () -> assertFalse(entry.getValue().getBoolean()));
+ }
+
+ @Test
+ void testAddString() {
+ NetworkTableEntry entry = m_tab.add("String", "foobar").getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/String", entry.getName()),
+ () -> assertEquals("foobar", entry.getValue().getString()));
+ }
+
+ @Test
+ void testAddNamedSendableWithProperties() {
+ Sendable sendable = new InstantCommand("Command");
+ String widgetType = "Command Widget";
+ m_tab.add(sendable)
+ .withWidget(widgetType)
+ .withProperties(mapOf("foo", 1234, "bar", "baz"));
+
+ m_instance.update();
+ String meta = "/Shuffleboard/.metadata/Tab/Command";
+
+ assertAll(
+ () -> assertEquals(1234,
+ m_ntInstance.getEntry(meta + "/Properties/foo").getDouble(-1),
+ "Property 'foo' not set correctly"),
+ () -> assertEquals("baz",
+ m_ntInstance.getEntry(meta + "/Properties/bar").getString(null),
+ "Property 'bar' not set correctly"),
+ () -> assertEquals(widgetType,
+ m_ntInstance.getEntry(meta + "/PreferredComponent").getString(null),
+ "Preferred component not set correctly"));
+ }
+
+ @Test
+ void testAddNumberArray() {
+ NetworkTableEntry entry = m_tab.add("DoubleArray", new double[]{1, 2, 3}).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/DoubleArray", entry.getName()),
+ () -> assertArrayEquals(new double[]{1, 2, 3}, entry.getValue().getDoubleArray()));
+ }
+
+ @Test
+ void testAddBooleanArray() {
+ NetworkTableEntry entry = m_tab.add("BoolArray", new boolean[]{true, false}).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/BoolArray", entry.getName()),
+ () -> assertArrayEquals(new boolean[]{true, false}, entry.getValue().getBooleanArray()));
+ }
+
+ @Test
+ void testAddStringArray() {
+ NetworkTableEntry entry = m_tab.add("StringArray", new String[]{"foo", "bar"}).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/StringArray", entry.getName()),
+ () -> assertArrayEquals(new String[]{"foo", "bar"}, entry.getValue().getStringArray()));
+ }
+
+ @Test
+ void testTitleDuplicates() {
+ m_tab.add("foo", "bar");
+ assertThrows(IllegalArgumentException.class, () -> m_tab.add("foo", "baz"));
+ }
+
+ /**
+ * Stub for Java 9 {@code Map.of()}.
+ */
+ @SuppressWarnings({"unchecked", "PMD"})
+ private static <K, V> Map<K, V> mapOf(Object... entries) {
+ Map<K, V> map = new HashMap<>();
+ for (int i = 0; i < entries.length; i += 2) {
+ map.put((K) entries[i], (V) entries[i + 1]);
+ }
+ return map;
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
new file mode 100644
index 0000000..c4285fb
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertSame;
+
+public class ShuffleboardTest extends UtilityClassTest {
+ public ShuffleboardTest() {
+ super(Shuffleboard.class);
+ }
+
+ // Most relevant tests are in ShuffleboardTabTest
+
+ @Test
+ void testTabObjectsCached() {
+ ShuffleboardTab tab1 = Shuffleboard.getTab("testTabObjectsCached");
+ ShuffleboardTab tab2 = Shuffleboard.getTab("testTabObjectsCached");
+ assertSame(tab1, tab2, "Tab objects were not cached");
+ }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
new file mode 100644
index 0000000..b739a92
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.sim;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogInSim;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class AnalogInputSimTest {
+ static class DoubleStore {
+ public boolean m_wasTriggered;
+ public boolean m_wasCorrectType;
+ public double m_setValue0;
+ }
+
+ @Test
+ void setCallbackTest() {
+ HAL.initialize(500, 0);
+
+ AnalogInput input = new AnalogInput(5);
+ AnalogInSim inputSim = input.getSimObject();
+
+ for (double i = 0; i < 5.0; i += 0.1) {
+ inputSim.setVoltage(0);
+
+ assertEquals(input.getVoltage(), 0, 0.001);
+
+ inputSim.setVoltage(i);
+
+ assertEquals(input.getVoltage(), i, 0.001);
+
+ }
+
+
+
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
new file mode 100644
index 0000000..258b253
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.sim;
+
+import edu.wpi.first.wpilibj.AnalogOutput;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.AnalogOutSim;
+import edu.wpi.first.hal.sim.CallbackStore;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class AnalogOutputSimTest {
+ static class DoubleStore {
+ public boolean m_wasTriggered;
+ public boolean m_wasCorrectType;
+ public double m_setValue = -1;
+
+ public void reset() {
+ m_wasCorrectType = false;
+ m_wasTriggered = false;
+ m_setValue = -1;
+ }
+ }
+
+ @Test
+ void setCallbackTest() {
+ HAL.initialize(500, 0);
+
+
+ AnalogOutput output = new AnalogOutput(0);
+ output.setVoltage(0.5);
+
+ AnalogOutSim outputSim = output.getSimObject();
+
+ DoubleStore store = new DoubleStore();
+
+ try (CallbackStore cb = outputSim.registerVoltageCallback((name, value) -> {
+ store.m_wasTriggered = true;
+ store.m_wasCorrectType = true;
+ store.m_setValue = value.getDouble();
+ }, false)) {
+ assertFalse(store.m_wasTriggered);
+
+ for (double i = 0.1; i < 5.0; i += 0.1) {
+ store.reset();
+
+ output.setVoltage(0);
+
+ assertTrue(store.m_wasTriggered);
+ assertEquals(store.m_setValue, 0, 0.001);
+
+ store.reset();
+
+ output.setVoltage(i);
+
+ assertTrue(store.m_wasTriggered);
+ assertEquals(store.m_setValue, i, 0.001);
+
+ }
+ }
+ }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
new file mode 100644
index 0000000..00dd002
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+class SmartDashboardTest extends UtilityClassTest {
+ SmartDashboardTest() {
+ super(SmartDashboard.class);
+ }
+}
diff --git a/wpilibj/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibj/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..f6d7b98
--- /dev/null
+++ b/wpilibj/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj.MockHardwareExtension