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..d901462
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * ADXL345 I2C Accelerometer.
+ */
+@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
+public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
+  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;
+
+  protected SimDevice m_simDevice;
+  protected SimEnum m_simRange;
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
+  /**
+   * 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);
+
+    // simulation
+    m_simDevice = SimDevice.create("ADXL345_I2C", port.value, deviceAddress);
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+      m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+    }
+
+    // Turn on the measurements
+    m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
+
+    setRange(range);
+
+    HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
+    SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_i2c != null) {
+      m_i2c.close();
+      m_i2c = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
+  }
+
+  @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);
+
+    if (m_simRange != null) {
+      m_simRange.set(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) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
+    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();
+    if (m_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
+    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..6aed40e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -0,0 +1,247 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * ADXL345 SPI Accelerometer.
+ */
+@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
+public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
+  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;
+
+  protected SimDevice m_simDevice;
+  protected SimEnum m_simRange;
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
+  /**
+   * 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);
+    // simulation
+    m_simDevice = SimDevice.create("ADXL345_SPI", port.value);
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+      m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+    }
+    init(range);
+    SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_spi != null) {
+      m_spi.close();
+      m_spi = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
+  }
+
+  /**
+   * 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);
+
+    if (m_simRange != null) {
+      m_simRange.set(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(ADXL345_SPI.Axes axis) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
+    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_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
+    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..54583ea
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -0,0 +1,272 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * ADXL362 SPI Accelerometer.
+ *
+ * <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
+ */
+@SuppressWarnings("PMD.UnusedPrivateField")
+public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
+  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 SimDevice m_simDevice;
+  private SimEnum m_simRange;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
+
+  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);
+
+    // simulation
+    m_simDevice = SimDevice.create("ADXL362", port.value);
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+      m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+    }
+
+    m_spi.setClockRate(3000000);
+    m_spi.setMSBFirst();
+    m_spi.setSampleDataOnTrailingEdge();
+    m_spi.setClockActiveLow();
+    m_spi.setChipSelectActiveLow();
+
+    ByteBuffer transferBuffer = ByteBuffer.allocate(3);
+    if (m_simDevice == null) {
+      // Validate the part ID
+      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 + 1);
+    SendableRegistry.addLW(this, "ADXL362", port.value);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_spi != null) {
+      m_spi.close();
+      m_spi = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = 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);
+
+    if (m_simRange != null) {
+      m_simRange.set(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 ADXL362 in Gs.
+   */
+  public double getAcceleration(ADXL362.Axes axis) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
+    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_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
+    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..549a91a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -0,0 +1,210 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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, AutoCloseable {
+  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;
+
+  private SimDevice m_simDevice;
+  private SimBoolean m_simConnected;
+  private SimDouble m_simAngle;
+  private SimDouble m_simRate;
+
+  /**
+   * 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);
+
+    // simulation
+    m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
+    if (m_simDevice != null) {
+      m_simConnected = m_simDevice.createBoolean("Connected", false, true);
+      m_simAngle = m_simDevice.createDouble("Angle", false, 0.0);
+      m_simRate = m_simDevice.createDouble("Rate", false, 0.0);
+    }
+
+    m_spi.setClockRate(3000000);
+    m_spi.setMSBFirst();
+    m_spi.setSampleDataOnLeadingEdge();
+    m_spi.setClockActiveHigh();
+    m_spi.setChipSelectActiveLow();
+
+    if (m_simDevice == null) {
+      // 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 + 1);
+    SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
+  }
+
+  /**
+   * Determine if the gyro is connected.
+   *
+   * @return true if it is connected, false otherwise.
+   */
+  public boolean isConnected() {
+    if (m_simConnected != null) {
+      return m_simConnected.get();
+    }
+    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_simAngle != null) {
+      m_simAngle.set(0.0);
+    }
+    if (m_simRate != null) {
+      m_simRate.set(0.0);
+    }
+    if (m_spi != null) {
+      m_spi.resetAccumulator();
+    }
+  }
+
+  /**
+   * Delete (free) the spi port used for the gyro and stop accumulating.
+   */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_spi != null) {
+      m_spi.close();
+      m_spi = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
+  }
+
+  @Override
+  public double getAngle() {
+    if (m_simAngle != null) {
+      return m_simAngle.get();
+    }
+    if (m_spi == null) {
+      return 0.0;
+    }
+    return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
+  }
+
+  @Override
+  public double getRate() {
+    if (m_simRate != null) {
+      return m_simRate.get();
+    }
+    if (m_spi == null) {
+      return 0.0;
+    }
+    return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
new file mode 100644
index 0000000..223c6f2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.AddressableLEDJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PWMJNI;
+
+/**
+ * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ */
+public class AddressableLED implements AutoCloseable {
+  private final int m_pwmHandle;
+  private final int m_handle;
+
+  /**
+   * Constructs a new driver for a specific port.
+   *
+   * @param port the output port to use (Must be a PWM port)
+   */
+  public AddressableLED(int port) {
+    m_pwmHandle = PWMJNI.initializePWMPort(HAL.getPort((byte) port));
+    m_handle = AddressableLEDJNI.initialize(m_pwmHandle);
+    HAL.report(tResourceType.kResourceType_AddressableLEDs, port + 1);
+  }
+
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      AddressableLEDJNI.free(m_handle);
+    }
+    if (m_pwmHandle != 0) {
+      PWMJNI.freePWMPort(m_pwmHandle);
+    }
+  }
+
+  /**
+   * Sets the length of the LED strip.
+   *
+   * <p>Calling this is an expensive call, so its best to call it once, then just update data.
+   *
+   * @param length the strip length
+   */
+  public void setLength(int length) {
+    AddressableLEDJNI.setLength(m_handle, length);
+  }
+
+  /**
+   * Sets the led output data.
+   *
+   * <p>If the output is enabled, this will start writing the next data cycle.
+   * It is safe to call, even while output is enabled.
+   *
+   * @param buffer the buffer to write
+   */
+  public void setData(AddressableLEDBuffer buffer) {
+    AddressableLEDJNI.setData(m_handle, buffer.m_buffer);
+  }
+
+  /**
+   * Sets the bit timing.
+   *
+   * <p>By default, the driver is set up to drive WS2812s, so nothing needs to be set for those.
+   *
+   * @param lowTime0NanoSeconds low time for 0 bit
+   * @param highTime0NanoSeconds high time for 0 bit
+   * @param lowTime1NanoSeconds low time for 1 bit
+   * @param highTime1NanoSeconds high time for 1 bit
+   */
+  public void setBitTiming(int lowTime0NanoSeconds, int highTime0NanoSeconds,
+      int lowTime1NanoSeconds, int highTime1NanoSeconds) {
+    AddressableLEDJNI.setBitTiming(m_handle, lowTime0NanoSeconds,
+        highTime0NanoSeconds, lowTime1NanoSeconds,
+        highTime1NanoSeconds);
+  }
+
+  /**
+   * Sets the sync time.
+   *
+   * <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812.
+   *
+   * @param syncTimeMicroSeconds the sync time
+   */
+  public void setSyncTime(int syncTimeMicroSeconds) {
+    AddressableLEDJNI.setSyncTime(m_handle, syncTimeMicroSeconds);
+  }
+
+  /**
+   * Starts the output.
+   *
+   * <p>The output writes continously.
+   */
+  public void start() {
+    AddressableLEDJNI.start(m_handle);
+  }
+
+  /**
+   * Stops the output.
+   */
+  public void stop() {
+    AddressableLEDJNI.stop(m_handle);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
new file mode 100644
index 0000000..3cc0763
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * Buffer storage for Addressable LEDs.
+ */
+public class AddressableLEDBuffer {
+  byte[] m_buffer;
+
+  /**
+   * Constructs a new LED buffer with the specified length.
+   *
+   * @param length The length of the buffer in pixels
+   */
+  public AddressableLEDBuffer(int length) {
+    m_buffer = new byte[length * 4];
+  }
+
+  /**
+   * Sets a specific led in the buffer.
+   *
+   * @param index the index to write
+   * @param r     the r value [0-255]
+   * @param g     the g value [0-255]
+   * @param b     the b value [0-255]
+   */
+  @SuppressWarnings("ParameterName")
+  public void setRGB(int index, int r, int g, int b) {
+    m_buffer[index * 4] = (byte) b;
+    m_buffer[(index * 4) + 1] = (byte) g;
+    m_buffer[(index * 4) + 2] = (byte) r;
+    m_buffer[(index * 4) + 3] = 0;
+  }
+
+  /**
+   * Sets a specific led in the buffer.
+   *
+   * @param index the index to write
+   * @param h     the h value [0-180]
+   * @param s     the s value [0-255]
+   * @param v     the v value [0-255]
+   */
+  @SuppressWarnings("ParameterName")
+  public void setHSV(final int index, final int h, final int s, final int v) {
+    if (s == 0) {
+      setRGB(index, v, v, v);
+      return;
+    }
+
+    final int region = h / 30;
+    final int remainder = (h - (region * 30)) * 6;
+
+    final int p = (v * (255 - s)) >> 8;
+    final int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+    final int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+    switch (region) {
+      case 0:
+        setRGB(index, v, t, p);
+        break;
+      case 1:
+        setRGB(index, q, v, p);
+        break;
+      case 2:
+        setRGB(index, p, v, t);
+        break;
+      case 3:
+        setRGB(index, p, q, v);
+        break;
+      case 4:
+        setRGB(index, t, p, v);
+        break;
+      default:
+        setRGB(index, v, p, q);
+        break;
+    }
+  }
+
+  /**
+   * Sets a specific LED in the buffer.
+   *
+   * @param index The index to write
+   * @param color The color of the LED
+   */
+  public void setLED(int index, Color color) {
+    setRGB(index,
+        (int) (color.red * 255),
+        (int) (color.green * 255),
+        (int) (color.blue * 255));
+  }
+
+  /**
+   * Sets a specific LED in the buffer.
+   *
+   * @param index The index to write
+   * @param color The color of the LED
+   */
+  public void setLED(int index, Color8Bit color) {
+    setRGB(index, color.red, color.green, color.blue);
+  }
+
+  /**
+   * Gets the buffer length.
+   *
+   * @return the buffer length
+   */
+  public int getLength() {
+    return m_buffer.length / 4;
+  }
+}
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..41c5b38
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 implements PIDSource, Sendable, AutoCloseable {
+  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() + 1);
+    SendableRegistry.addLW(this, "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);
+    SendableRegistry.addChild(this, 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) {
+    requireNonNullParam(channel, "channel", "AnalogAccelerometer");
+    m_allocatedChannel = allocatedChannel;
+    m_analogChannel = channel;
+    initAccelerometer();
+  }
+
+  /**
+   * Delete the analog components used for the accelerometer.
+   */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    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/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
new file mode 100644
index 0000000..75177e0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for supporting continuous analog encoders, such as the US Digital MA3.
+ */
+public class AnalogEncoder implements Sendable, AutoCloseable {
+  private final AnalogInput m_analogInput;
+  private AnalogTrigger m_analogTrigger;
+  private Counter m_counter;
+  private double m_positionOffset;
+  private double m_distancePerRotation = 1.0;
+  private double m_lastPosition;
+
+  protected SimDevice m_simDevice;
+  protected SimDouble m_simPosition;
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  public AnalogEncoder(AnalogInput analogInput) {
+    m_analogInput = analogInput;
+    init();
+  }
+
+  private void init() {
+    m_analogTrigger = new AnalogTrigger(m_analogInput);
+    m_counter = new Counter();
+
+    m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
+
+    if (m_simDevice != null) {
+      m_simPosition = m_simDevice.createDouble("Position", false, 0.0);
+    }
+
+    // Limits need to be 25% from each end
+    m_analogTrigger.setLimitsVoltage(1.25, 3.75);
+    m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
+    m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
+
+    SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel());
+  }
+
+  /**
+   * Get the encoder value since the last reset.
+   *
+   * <p>This is reported in rotations since the last reset.
+   *
+   * @return the encoder value in rotations
+   */
+  public double get() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    // As the values are not atomic, keep trying until we get 2 reads of the same
+    // value. If we don't within 10 attempts, warn
+    for (int i = 0; i < 10; i++) {
+      double counter = m_counter.get();
+      double pos = m_analogInput.getVoltage();
+      double counter2 = m_counter.get();
+      double pos2 = m_analogInput.getVoltage();
+      if (counter == counter2 && pos == pos2) {
+        double position = counter + pos - m_positionOffset;
+        m_lastPosition = position;
+        return position;
+      }
+    }
+
+    DriverStation.reportWarning(
+        "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
+    return m_lastPosition;
+  }
+
+  /**
+   * Get the offset of position relative to the last reset.
+   *
+   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute
+   * position relative to the last reset. This could potentially be negative,
+   * which needs to be accounted for.
+   *
+   * @return the position offset
+   */
+  public double getPositionOffset() {
+    return m_positionOffset;
+  }
+
+  /**
+   * Set the distance per rotation of the encoder. This sets the multiplier used
+   * to determine the distance driven based on the rotation value from the
+   * encoder. Set this value based on the how far the mechanism travels in 1
+   * rotation of the encoder, and factor in gearing reductions following the
+   * encoder shaft. This distance can be in any units you like, linear or angular.
+   *
+   * @param distancePerRotation the distance per rotation of the encoder
+   */
+  public void setDistancePerRotation(double distancePerRotation) {
+    m_distancePerRotation = distancePerRotation;
+  }
+
+  /**
+   * Get the distance per rotation for this encoder.
+   *
+   * @return The scale factor that will be used to convert rotation to useful
+   *         units.
+   */
+  public double getDistancePerRotation() {
+    return m_distancePerRotation;
+  }
+
+  /**
+   * Get the distance the sensor has driven since the last reset as scaled by the
+   * value from {@link #setDistancePerRotation(double)}.
+   *
+   * @return The distance driven since the last reset
+   */
+  public double getDistance() {
+    return get() * getDistancePerRotation();
+  }
+
+  /**
+   * Reset the Encoder distance to zero.
+   */
+  public void reset() {
+    m_counter.reset();
+    m_positionOffset = m_analogInput.getVoltage();
+  }
+
+  @Override
+  public void close() {
+    m_counter.close();
+    m_analogTrigger.close();
+    if (m_simDevice != null) {
+      m_simDevice.close();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("AbsoluteEncoder");
+    builder.addDoubleProperty("Distance", this::getDistance, null);
+    builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, 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..c688b55
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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, AutoCloseable {
+  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() + 1);
+    SendableRegistry.addLW(this, "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;
+    SendableRegistry.addChild(this, 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) {
+    requireNonNullParam(channel, "channel", "AnalogGyro");
+
+    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;
+    SendableRegistry.addChild(this, 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) {
+    requireNonNullParam(channel, "channel", "AnalogGyro");
+
+    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() {
+    SendableRegistry.remove(this);
+    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..308be6b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -0,0 +1,367 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SimDevice;
+import edu.wpi.first.hal.sim.AnalogInSim;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements PIDSource, Sendable, AutoCloseable {
+  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 + 1);
+    SendableRegistry.addLW(this, "AnalogInput", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    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();
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    AnalogJNI.setAnalogInputSimDevice(m_port, device.getNativeHandle());
+  }
+
+  @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..ae03463
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Analog output class.
+ */
+public class AnalogOutput implements Sendable, AutoCloseable {
+  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 + 1);
+    SendableRegistry.addLW(this, "AnalogOutput", channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    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..3a6f338
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -0,0 +1,167 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Potentiometer, Sendable, AutoCloseable {
+  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;
+    SendableRegistry.addChild(this, 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) {
+    SendableRegistry.addLW(this, "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() {
+    SendableRegistry.remove(this);
+    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..f7f98bd
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -0,0 +1,210 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.util.BoundaryException;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+
+/**
+ * Class for creating and configuring Analog Triggers.
+ */
+public class AnalogTrigger implements Sendable, AutoCloseable {
+  /**
+   * 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 AnalogInput m_analogInput;
+  protected DutyCycle m_dutyCycle;
+  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;
+    SendableRegistry.addChild(this, 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;
+
+    m_port = AnalogJNI.initializeAnalogTrigger(channel.m_port);
+
+    int index = getIndex();
+
+    HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
+    SendableRegistry.addLW(this, "AnalogTrigger", index);
+  }
+
+  /**
+   * Construct an analog trigger given a duty cycle input.
+   *
+   * @param input the DutyCycle to use for the analog trigger
+   */
+  public AnalogTrigger(DutyCycle input) {
+    m_dutyCycle = input;
+
+    m_port = AnalogJNI.initializeAnalogTriggerDutyCycle(input.m_handle);
+
+    int index = getIndex();
+
+    HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
+    SendableRegistry.addLW(this, "AnalogTrigger", index);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    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
+   * values between 0 and 1.
+   *
+   * @param lower the lower duty cycle limit
+   * @param upper the upper duty cycle limit
+   */
+  public void setLimitsDutyCycle(double lower, double upper) {
+    if (lower > upper) {
+      throw new BoundaryException("Lower bound is greater than upper bound");
+    }
+    AnalogJNI.setAnalogTriggerLimitsDutyCycle(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 final int getIndex() {
+    return AnalogJNI.getAnalogTriggerFPGAIndex(m_port);
+  }
+
+  /**
+   * 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..7635196
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 implements Sendable {
+  /**
+   * 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) {
+    requireNonNullParam(trigger, "trigger", "AnalogTriggerOutput");
+    requireNonNullParam(outputType, "outputType", "AnalogTriggerOutput");
+
+    m_trigger = trigger;
+    m_outputType = outputType;
+    HAL.report(tResourceType.kResourceType_AnalogTriggerOutput,
+        trigger.getIndex() + 1, outputType.value + 1);
+  }
+
+  /**
+   * 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.getIndex();
+  }
+
+  @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..f6811c0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Built-in accelerometer.
+ *
+ * <p>This class allows access to the roboRIO's internal accelerometer.
+ */
+public class BuiltInAccelerometer implements Accelerometer, Sendable, AutoCloseable {
+  /**
+   * 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");
+    SendableRegistry.addLW(this, "BuiltInAccel", 0);
+  }
+
+  /**
+   * Constructor. The accelerometer will measure +/-8 g-forces
+   */
+  public BuiltInAccelerometer() {
+    this(Range.k8G);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  @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..7b188e5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 + 1);
+  }
+
+  /**
+   * 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 + 1);
+  }
+
+  /**
+   * 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);
+  }
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10 bits.
+   * The length by spec must match what is returned by the responding device
+   *
+   * @param length The length to request (0 to 8)
+   * @param apiId The API ID to write.
+   */
+  public void writeRTRFrame(int length, int apiId) {
+    CANAPIJNI.writeCANRTRFrame(m_handle, length, apiId);
+  }
+
+  /**
+   * 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);
+  }
+}
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..178d5ee
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
@@ -0,0 +1,784 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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];
+    }
+
+    if (CameraServerSharedStore.getCameraServerShared().isRoboRIO()) {
+      // Look to see if we have a passthrough server for this source
+      // Only do this on the roboRIO
+      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/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
new file mode 100644
index 0000000..9d8a8dd
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  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 + 1);
+    SendableRegistry.addLW(this, "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());
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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..31d50d8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.
+ *
+ * @deprecated None of the 2020 FRC controllers use this.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+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/Counter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
new file mode 100644
index 0000000..0a26810
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -0,0 +1,566 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+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 implements CounterBase, PIDSource, Sendable, AutoCloseable {
+  /**
+   * 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(0.5);
+
+    HAL.report(tResourceType.kResourceType_Counter, m_index + 1, mode.value + 1);
+    SendableRegistry.addLW(this, "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();
+
+    requireNonNullParam(source, "source", "Counter");
+    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);
+
+    requireNonNullParam(encodingType, "encodingType", "Counter");
+    requireNonNullParam(upSource, "upSource", "Counter");
+    requireNonNullParam(downSource, "downSource", "Counter");
+
+    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();
+
+    requireNonNullParam(trigger, "trigger", "Counter");
+
+    setUpSource(trigger.createOutput(AnalogTriggerType.kState));
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+
+    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;
+    SendableRegistry.addChild(this, 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) {
+    requireNonNullParam(analogTrigger, "analogTrigger", "setUpSource");
+    requireNonNullParam(triggerType, "triggerType", "setUpSource");
+
+    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;
+    SendableRegistry.addChild(this, 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) {
+    requireNonNullParam(analogTrigger, "analogTrigger", "setDownSource");
+    requireNonNullParam(triggerType, "analogTrigger", "setDownSource");
+
+    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) {
+    requireNonNullParam(pidSource, "pidSource", "setPIDSourceType");
+    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..0feeebb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * Digilent DMC 60 Speed Controller.
+ *
+ * <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><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class DMC60 extends PWMSpeedController {
+  /**
+   * Constructor.
+   *
+   * @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, 0.997);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel() + 1);
+    SendableRegistry.setName(this, "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..10155cc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  /**
+   * 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 + 1, 0);
+        SendableRegistry.addLW(this, "DigitalGlitchFilter", index);
+      }
+    }
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    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..7cbd1ed
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.hal.SimDevice;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  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 + 1);
+    SendableRegistry.addLW(this, "DigitalInput", channel);
+  }
+
+  @Override
+  public void close() {
+    super.close();
+    SendableRegistry.remove(this);
+    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;
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    DIOJNI.setDIOSimDevice(m_handle, device.getNativeHandle());
+  }
+
+  @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..ec1a44c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.hal.SimDevice;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 DigitalSource implements Sendable, AutoCloseable {
+  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 + 1);
+    SendableRegistry.addLW(this, "DigitalOutput", channel);
+  }
+
+  @Override
+  public void close() {
+    super.close();
+    SendableRegistry.remove(this);
+    // 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.
+   */
+  @Override
+  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);
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    DIOJNI.setDIOSimDevice(m_handle, device.getNativeHandle());
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Digital Output");
+    builder.addBooleanProperty("Value", this::get, this::set);
+  }
+
+  /**
+   * Is this an analog trigger.
+   *
+   * @return true if this is an analog trigger
+   */
+  @Override
+  public boolean isAnalogTrigger() {
+    return false;
+  }
+
+  /**
+   * Get the analog trigger type.
+   *
+   * @return false
+   */
+  @Override
+  public int getAnalogTriggerTypeForRouting() {
+    return 0;
+  }
+
+  /**
+   * Get the HAL Port Handle.
+   *
+   * @return The HAL Handle to the specified source.
+   */
+  @Override
+  public int getPortHandleForRouting() {
+    return m_handle;
+  }
+}
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..d612d24
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  /**
+   * 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 + 1,
+                                   m_moduleNumber + 1);
+    HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel + 1,
+                                   m_moduleNumber + 1);
+    SendableRegistry.addLW(this, "DoubleSolenoid", m_moduleNumber, forwardChannel);
+  }
+
+  @Override
+  public synchronized void close() {
+    SendableRegistry.remove(this);
+    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..997fe2b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -0,0 +1,1150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.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];
+      for (int i = 0; i < count; i++) {
+        m_povs[i] = -1;
+      }
+    }
+  }
+
+  /**
+   * 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");
+        return -1;
+      }
+    } 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 Robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped, false otherwise.
+   */
+  public boolean isEStopped() {
+    synchronized (m_controlWordMutex) {
+      updateControlWord(false);
+      return m_controlWordCache.getEStop();
+    }
+  }
+
+  /**
+   * 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();
+    }
+  }
+
+  /**
+   * 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();
+  }
+
+  /**
+   * 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());
+  }
+
+  /**
+   * Forces waitForData() to return immediately.
+   */
+  public void wakeupWaitForData() {
+    m_waitForDataMutex.lock();
+    m_waitForDataCount++;
+    m_waitForDataCond.signalAll();
+    m_waitForDataMutex.unlock();
+  }
+
+  /**
+   * 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();
+    }
+
+    wakeupWaitForData();
+    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/DutyCycle.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
new file mode 100644
index 0000000..0834fad
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.DutyCycleJNI;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to low
+ * in that frequency. There are 8 of these in the roboRIO, and they can be
+ * attached to any {@link DigitalSource}.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in order
+ * to implement rollover checking.
+ *
+ */
+public class DutyCycle implements Sendable, AutoCloseable {
+  // Explicitly package private
+  final int m_handle;
+
+  private final DigitalSource m_source;
+
+  /**
+   * Constructs a DutyCycle input from a DigitalSource input.
+   *
+   * <p>This class does not own the inputted source.
+   *
+   * @param digitalSource The DigitalSource to use.
+   */
+  public DutyCycle(DigitalSource digitalSource) {
+    m_handle = DutyCycleJNI.initialize(digitalSource.getPortHandleForRouting(),
+        digitalSource.getAnalogTriggerTypeForRouting());
+
+    m_source = digitalSource;
+    int index = getFPGAIndex();
+    HAL.report(tResourceType.kResourceType_DutyCycle, index + 1);
+    SendableRegistry.addLW(this, "Duty Cycle", index);
+  }
+
+  /**
+   * Close the DutyCycle and free all resources.
+   */
+  @Override
+  public void close() {
+    DutyCycleJNI.free(m_handle);
+  }
+
+  /**
+   * Get the frequency of the duty cycle signal.
+   *
+   * @return frequency in Hertz
+   */
+  public int getFrequency() {
+    return DutyCycleJNI.getFrequency(m_handle);
+  }
+
+  /**
+   * Get the output ratio of the duty cycle signal.
+   *
+   * <p>0 means always low, 1 means always high.
+   *
+   * @return output ratio between 0 and 1
+   */
+  public double getOutput() {
+    return DutyCycleJNI.getOutput(m_handle);
+  }
+
+  /**
+   * Get the raw output ratio of the duty cycle signal.
+   *
+   * <p>0 means always low, an output equal to getOutputScaleFactor() means always
+   * high.
+   *
+   * @return output ratio in raw units
+   */
+  public int getOutputRaw() {
+    return DutyCycleJNI.getOutputRaw(m_handle);
+  }
+
+  /**
+   * Get the scale factor of the output.
+   *
+   * <p>An output equal to this value is always high, and then linearly scales down
+   * to 0. Divide the result of getOutputRaw by this in order to get the
+   * percentage between 0 and 1.
+   *
+   * @return the output scale factor
+   */
+  public int getOutputScaleFactor() {
+    return DutyCycleJNI.getOutputScaleFactor(m_handle);
+  }
+
+  /**
+   * Get the FPGA index for the DutyCycle.
+   *
+   * @return the FPGA index
+   */
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public final int getFPGAIndex() {
+    return DutyCycleJNI.getFPGAIndex(m_handle);
+  }
+
+  public int getSourceChannel() {
+    return m_source.getChannel();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Duty Cycle");
+    builder.addDoubleProperty("Frequency", this::getFrequency, null);
+    builder.addDoubleProperty("Output", this::getOutput, null);
+
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
new file mode 100644
index 0000000..b2040f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
@@ -0,0 +1,229 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with
+ * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ * Encoder.
+ */
+public class DutyCycleEncoder implements Sendable, AutoCloseable {
+  private final DutyCycle m_dutyCycle;
+  private boolean m_ownsDutyCycle;
+  private AnalogTrigger m_analogTrigger;
+  private Counter m_counter;
+  private int m_frequencyThreshold = 100;
+  private double m_positionOffset;
+  private double m_distancePerRotation = 1.0;
+  private double m_lastPosition;
+
+  protected SimDevice m_simDevice;
+  protected SimDouble m_simPosition;
+  protected SimBoolean m_simIsConnected;
+
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * @param channel the channel to attach to
+   */
+  public DutyCycleEncoder(int channel) {
+    m_dutyCycle = new DutyCycle(new DigitalInput(channel));
+    init();
+  }
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  public DutyCycleEncoder(DutyCycle dutyCycle) {
+    m_dutyCycle = dutyCycle;
+    init();
+  }
+
+  /**
+   * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+   *
+   * @param source the digital source to attach to
+   */
+  public DutyCycleEncoder(DigitalSource source) {
+    m_ownsDutyCycle = true;
+    m_dutyCycle = new DutyCycle(source);
+    init();
+  }
+
+  private void init() {
+    m_analogTrigger = new AnalogTrigger(m_dutyCycle);
+    m_counter = new Counter();
+
+    m_simDevice = SimDevice.create("DutyCycleEncoder", m_dutyCycle.getFPGAIndex());
+
+    if (m_simDevice != null) {
+      m_simPosition = m_simDevice.createDouble("Position", false, 0.0);
+      m_simIsConnected = m_simDevice.createBoolean("Connected", false, true);
+    }
+
+    m_analogTrigger.setLimitsDutyCycle(0.25, 0.75);
+    m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
+    m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
+
+    SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
+  }
+
+  /**
+   * Get the encoder value since the last reset.
+   *
+   * <p>This is reported in rotations since the last reset.
+   *
+   * @return the encoder value in rotations
+   */
+  public double get() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    // As the values are not atomic, keep trying until we get 2 reads of the same
+    // value
+    // If we don't within 10 attempts, error
+    for (int i = 0; i < 10; i++) {
+      double counter = m_counter.get();
+      double pos = m_dutyCycle.getOutput();
+      double counter2 = m_counter.get();
+      double pos2 = m_dutyCycle.getOutput();
+      if (counter == counter2 && pos == pos2) {
+        double position = counter + pos - m_positionOffset;
+        m_lastPosition = position;
+        return position;
+      }
+    }
+
+    DriverStation.reportWarning(
+        "Failed to read Analog Encoder. Potential Speed Overrun. Returning last value", false);
+    return m_lastPosition;
+  }
+
+  /**
+   * Get the offset of position relative to the last reset.
+   *
+   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute
+   * position relative to the last reset. This could potentially be negative,
+   * which needs to be accounted for.
+   *
+   * @return the position offset
+   */
+  public double getPositionOffset() {
+    return m_positionOffset;
+  }
+
+  /**
+   * Set the distance per rotation of the encoder. This sets the multiplier used
+   * to determine the distance driven based on the rotation value from the
+   * encoder. Set this value based on the how far the mechanism travels in 1
+   * rotation of the encoder, and factor in gearing reductions following the
+   * encoder shaft. This distance can be in any units you like, linear or angular.
+   *
+   * @param distancePerRotation the distance per rotation of the encoder
+   */
+  public void setDistancePerRotation(double distancePerRotation) {
+    m_distancePerRotation = distancePerRotation;
+  }
+
+  /**
+   * Get the distance per rotation for this encoder.
+   *
+   * @return The scale factor that will be used to convert rotation to useful
+   *         units.
+   */
+  public double getDistancePerRotation() {
+    return m_distancePerRotation;
+  }
+
+  /**
+   * Get the distance the sensor has driven since the last reset as scaled by the
+   * value from {@link #setDistancePerRotation(double)}.
+   *
+   * @return The distance driven since the last reset
+   */
+  public double getDistance() {
+    return get() * getDistancePerRotation();
+  }
+
+  /**
+   * Get the frequency in Hz of the duty cycle signal from the encoder.
+   *
+   * @return duty cycle frequency in Hz
+   */
+  public int getFrequency() {
+    return m_dutyCycle.getFrequency();
+  }
+
+  /**
+   * Reset the Encoder distance to zero.
+   */
+  public void reset() {
+    m_counter.reset();
+    m_positionOffset = m_dutyCycle.getOutput();
+  }
+
+  /**
+   * Get if the sensor is connected
+   *
+   * <p>This uses the duty cycle frequency to determine if the sensor is connected.
+   * By default, a value of 100 Hz is used as the threshold, and this value can be
+   * changed with {@link #setConnectedFrequencyThreshold(int)}.
+   *
+   * @return true if the sensor is connected
+   */
+  public boolean isConnected() {
+    if (m_simIsConnected != null) {
+      return m_simIsConnected.get();
+    }
+    return getFrequency() > m_frequencyThreshold;
+  }
+
+  /**
+   * Change the frequency threshold for detecting connection used by
+   * {@link #isConnected()}.
+   *
+   * @param frequency the minimum frequency in Hz.
+   */
+  public void setConnectedFrequencyThreshold(int frequency) {
+    if (frequency < 0) {
+      frequency = 0;
+    }
+
+    m_frequencyThreshold = frequency;
+  }
+
+  @Override
+  public void close() {
+    m_counter.close();
+    m_analogTrigger.close();
+    if (m_ownsDutyCycle) {
+      m_dutyCycle.close();
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("AbsoluteEncoder");
+    builder.addDoubleProperty("Distance", this::getDistance, null);
+    builder.addDoubleProperty("Distance Per Rotation", this::getDistancePerRotation, null);
+    builder.addBooleanProperty("Is Connected", this::isConnected, null);
+  }
+}
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..125f769
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -0,0 +1,586 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SimDevice;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 implements CounterBase, PIDSource, Sendable, AutoCloseable {
+  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 + 1, type.value + 1);
+    SendableRegistry.addLW(this, "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) {
+    requireNonNullParam(encodingType, "encodingType", "Encoder");
+
+    m_allocatedA = true;
+    m_allocatedB = true;
+    m_allocatedI = false;
+    m_aSource = new DigitalInput(channelA);
+    m_bSource = new DigitalInput(channelB);
+    SendableRegistry.addChild(this, m_aSource);
+    SendableRegistry.addChild(this, 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);
+    SendableRegistry.addChild(this, 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) {
+    requireNonNullParam(sourceA, "sourceA", "Encoder");
+    requireNonNullParam(sourceB, "sourceB", "Encoder");
+    requireNonNullParam(encodingType, "encodingType", "Encoder");
+
+    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() {
+    SendableRegistry.remove(this);
+    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;
+    SendableRegistry.addChild(this, 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);
+  }
+
+  /**
+   * Indicates this input is used by a simulated device.
+   *
+   * @param device simulated device handle
+   */
+  public void setSimDevice(SimDevice device) {
+    EncoderJNI.setEncoderSimDevice(m_encoder, device.getNativeHandle());
+  }
+
+  @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/GearTooth.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
new file mode 100644
index 0000000..41f9d09
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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.
+ *
+ * @deprecated The only sensor this works with is no longer available and no teams use it according
+ *             to FMS usage reporting.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+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 + 1, 0, "D");
+    } else {
+      HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0);
+    }
+    SendableRegistry.setName(this, "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() + 1, 0, "D");
+    } else {
+      HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0);
+    }
+    SendableRegistry.setName(this, "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..21330ce
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 implements Gyro, PIDSource, Sendable {
+  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..ca8374a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
@@ -0,0 +1,374 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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);
+  }
+
+  @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. If you intend to write multiple bytes in the same transaction and do not
+   * plan to receive anything back, use writeBulk() instead. Calling this with a receiveSize
+   * of 0 will result in an error.
+   *
+   * @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) {
+    requireNonNullParam(buffer, "buffer", "read");
+
+    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) {
+    requireNonNullParam(buffer, "buffer", "readOnly");
+    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..bd21d24
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
@@ -0,0 +1,284 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.function.Consumer;
+
+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 implements AutoCloseable {
+  @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;
+    }
+
+    public static WaitResult getValue(boolean rising, boolean falling) {
+      if (rising && falling) {
+        return kBoth;
+      } else if (rising) {
+        return kRisingEdge;
+      } else if (falling) {
+        return kFallingEdge;
+      } else {
+        return kTimeout;
+      }
+    }
+  }
+
+  /**
+   * 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() {
+    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 Consumer} 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(Consumer<WaitResult> 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, (mask, obj) -> {
+      // Rising edge result is the interrupt bit set in the byte 0xFF
+      // Falling edge result is the interrupt bit set in the byte 0xFF00
+      boolean rising = (mask & 0xFF) != 0;
+      boolean falling = (mask & 0xFF00) != 0;
+      handler.accept(WaitResult.getValue(rising, falling));
+    }, null);
+  }
+
+  /**
+   * 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
+    boolean rising = (result & 0xFF) != 0;
+    boolean falling = (result & 0xFF00) != 0;
+    return WaitResult.getValue(rising, falling);
+  }
+
+  /**
+   * 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..f1baefb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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;
+  private volatile boolean m_exit;
+
+  /**
+   * 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 (!Thread.currentThread().isInterrupted()) {
+      // Wait for new data to arrive
+      m_ds.waitForData();
+      if (m_exit) {
+        break;
+      }
+
+      loopFunc();
+    }
+  }
+
+  /**
+   * Ends the main loop in startCompetition().
+   */
+  @Override
+  public void endCompetition() {
+    m_exit = true;
+    m_ds.wakeupWaitForData();
+  }
+}
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..b9a7a6a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
@@ -0,0 +1,278 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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... Override 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... Override 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... Override 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... Override 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... Override 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... Override 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... Override 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... Override 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... Override 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... Override 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()");
+
+    SmartDashboard.updateValues();
+    m_watchdog.addEpoch("SmartDashboard.updateValues()");
+    LiveWindow.updateValues();
+    m_watchdog.addEpoch("LiveWindow.updateValues()");
+    Shuffleboard.update();
+    m_watchdog.addEpoch("Shuffleboard.update()");
+    m_watchdog.disable();
+
+    // 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..8ce7b7d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device.
+ *
+ * <p>Note that the Jaguar 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 Jaguar User Manual
+ * available from Vex.
+ *
+ * <p><ul>
+ * <li>2.310ms = full "forward"
+ * <li>1.550ms = the "high end" of the deadband range
+ * <li>1.507ms = center of the deadband range (off)
+ * <li>1.454ms = the "low end" of the deadband range
+ * <li>0.697ms = full "reverse"
+ * </ul>
+ */
+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);
+
+    setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_Jaguar, getChannel() + 1);
+    SendableRegistry.setName(this, "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..60a60bb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -0,0 +1,327 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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;
+
+  /**
+   * 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 + 1);
+  }
+
+  /**
+   * 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;
+  }
+
+  /**
+   * 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 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]);
+  }
+
+  /**
+   * 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 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/LinearFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
new file mode 100644
index 0000000..94bcb31
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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 edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * 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, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
+ * Notifier for this 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 calculate() gets called at the desired, constant frequency!
+ */
+public class LinearFilter {
+  private final CircularBuffer m_inputs;
+  private final CircularBuffer m_outputs;
+  private final double[] m_inputGains;
+  private final double[] m_outputGains;
+
+  private static int instances;
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feed forward" or FIR gains.
+   * @param fbGains The "feed back" or IIR gains.
+   */
+  public LinearFilter(double[] ffGains, double[] fbGains) {
+    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 timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the user.
+   */
+  public static LinearFilter singlePoleIIR(double timeConstant,
+                                           double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {1.0 - gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(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 timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the user.
+   */
+  public static LinearFilter highPass(double timeConstant,
+                                      double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {gain, -gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(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 taps The number of samples to average over. Higher = smoother but slower.
+   * @throws IllegalArgumentException if number of taps is less than 1.
+   */
+  public static LinearFilter movingAverage(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 LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Reset the filter state.
+   */
+  public void reset() {
+    m_inputs.clear();
+    m_outputs.clear();
+  }
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   *
+   * @return The filtered value at this step
+   */
+  public double calculate(double input) {
+    double retVal = 0.0;
+
+    // Rotate the inputs
+    m_inputs.addFirst(input);
+
+    // 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/MedianFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
new file mode 100644
index 0000000..a4a3a95
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.Collections;
+import java.util.List;
+
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing measurement noise,
+ * especially with processes that generate occasional, extreme outliers (such as values from
+ * vision processing, LIDAR, or ultrasonic sensors).
+ */
+public class MedianFilter {
+  private final CircularBuffer m_valueBuffer;
+  private final List<Double> m_orderedValues;
+  private final int m_size;
+
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  public MedianFilter(int size) {
+    // Circular buffer of values currently in the window, ordered by time
+    m_valueBuffer = new CircularBuffer(size);
+    // List of values currently in the window, ordered by value
+    m_orderedValues = new ArrayList<>(size);
+    // Size of rolling window
+    m_size = size;
+  }
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  public double calculate(double next) {
+    // Find insertion point for next value
+    int index = Collections.binarySearch(m_orderedValues, next);
+
+    // Deal with binarySearch behavior for element not found
+    if (index < 0) {
+      index = Math.abs(index + 1);
+    }
+
+    // Place value at proper insertion point
+    m_orderedValues.add(index, next);
+
+    int curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.remove(m_valueBuffer.removeLast());
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.addFirst(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues.get(curSize / 2);
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  public void reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.clear();
+  }
+}
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/NidecBrushless.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
new file mode 100644
index 0000000..9d1011b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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;
+
+  /**
+   * 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) {
+    setSafetyEnabled(false);
+
+    // the dio controls the output (in PWM mode)
+    m_dio = new DigitalOutput(dioChannel);
+    SendableRegistry.addChild(this, m_dio);
+    m_dio.setPWMRate(15625);
+    m_dio.enablePWM(0.5);
+
+    // the pwm enables the controller
+    m_pwm = new PWM(pwmChannel);
+    SendableRegistry.addChild(this, m_pwm);
+
+    HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
+    SendableRegistry.addLW(this, "Nidec Brushless", pwmChannel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_dio.close();
+    m_pwm.close();
+  }
+
+  /**
+   * 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..806bfa9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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;
+
+import static java.util.Objects.requireNonNull;
+
+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) {
+    requireNonNull(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();
+  }
+
+  /**
+   * Sets the name of the notifier.  Used for debugging purposes only.
+   *
+   * @param name Name
+   */
+  public void setName(String name) {
+    m_thread.setName(name);
+    NotifierJNI.setNotifierName(m_notifier.get(), name);
+  }
+
+  /**
+   * 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..40eb42f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
@@ -0,0 +1,829 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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.
+ *
+ * @deprecated All APIs which use this have been deprecated.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+@SuppressWarnings("PMD.TooManyFields")
+public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
+  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 LinearFilter 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) {
+    requireNonNullParam(source, "PIDSource", "PIDBase");
+    requireNonNullParam(output, "output", "PIDBase");
+
+    m_setpointTimer = new Timer();
+    m_setpointTimer.start();
+
+    m_P = Kp;
+    m_I = Ki;
+    m_D = Kd;
+    m_F = Kf;
+
+    m_pidInput = source;
+    m_filter = LinearFilter.movingAverage(1);
+
+    m_pidOutput = output;
+
+    instances++;
+    HAL.report(tResourceType.kResourceType_PIDController, instances);
+    m_tolerance = new NullTolerance();
+    SendableRegistry.add(this, "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);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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_pidInput == 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_filter.calculate(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_filter.calculate(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
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidInput.setPIDSourceType(pidSource);
+  }
+
+  /**
+   * Returns the type of input the PID controller is using.
+   *
+   * @return the PID controller input type
+   */
+  public 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 LinearFilter as the input.
+   * @param bufLength Number of previous cycles to average.
+   */
+  @Deprecated
+  public void setToleranceBuffer(int bufLength) {
+    m_thisMutex.lock();
+    try {
+      m_filter = LinearFilter.movingAverage(bufLength);
+    } 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..881723e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.controller.PIDController} instead.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public class PIDController extends PIDBase implements Controller, AutoCloseable {
+  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() {
+    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..10e60d3
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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;
+
+@Deprecated(since = "2020", forRemoval = true)
+@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..e9af2ff
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.
+ *
+ * @deprecated Use DoubleConsumer and new PIDController class.
+ */
+@FunctionalInterface
+@Deprecated(since = "2020", forRemoval = true)
+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..3c4f1f5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.
+ *
+ * @deprecated Use DoubleSupplier and new PIDController class.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+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..7508be5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+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..8febd2c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
@@ -0,0 +1,265 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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;
+
+  // Package private to use from AddressableLED
+  int m_handle;
+
+  /**
+   * 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) {
+    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 + 1);
+    SendableRegistry.addLW(this, "PWM", channel);
+
+    setSafetyEnabled(false);
+  }
+
+  /**
+   * Free the resource associated with the PWM channel and set the value to 0.
+   */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    if (m_handle == 0) {
+      return;
+    }
+    setDisabled();
+    PWMJNI.freePWMPort(m_handle);
+    m_handle = 0;
+  }
+
+  @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/PWMSparkMax.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
new file mode 100644
index 0000000..0f97ea5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.SendableRegistry;
+
+/**
+ * REV Robotics SPARK MAX Speed Controller with PWM control.
+ *
+ * <P>Note that the SPARK MAX 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 MAX User Manual
+ * available from REV Robotics.
+ *
+ * <p><ul>
+ * <li> 2.003ms = full "forward"
+ * <li> 1.550ms = the "high end" of the deadband range
+ * <li> 1.500ms = center of the deadband range (off)
+ * <li> 1.460ms = the "low end" of the deadband range
+ * <li> 0.999ms = full "reverse"
+ * </ul>
+ */
+public class PWMSparkMax extends PWMSpeedController {
+  /**
+   * Common initialization code called by all constructors.
+   *
+   */
+  public PWMSparkMax(final int channel) {
+    super(channel);
+
+    setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel() + 1);
+    SendableRegistry.setName(this, "PWMSparkMax", getChannel());
+  }
+}
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/PWMTalonFX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
new file mode 100644
index 0000000..03acab2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM control.
+ *
+ * <p>Note that the TalonFX 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 TalonFX User Manual
+ * available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonFX extends PWMSpeedController {
+  /**
+   * Constructor for a TalonFX connected via PWM.
+   *
+   * @param channel The PWM channel that the Talon FX is attached to. 0-9 are on-board, 10-19 are
+   *                on the MXP port
+   */
+  public PWMTalonFX(final int channel) {
+    super(channel);
+
+    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_TalonFX, getChannel() + 1);
+    SendableRegistry.setName(this, "PWMTalonFX", getChannel());
+  }
+}
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..1859e7f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control.
+ *
+ * <p>Note that the TalonSRX 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><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonSRX extends PWMSpeedController {
+  /**
+   * Constructor for a TalonSRX connected via PWM.
+   *
+   * @param channel The PWM channel that the Talon SRX 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, 0.997);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel() + 1);
+    SendableRegistry.setName(this, "PWMTalonSRX", getChannel());
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
new file mode 100644
index 0000000..9a1116d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.SendableRegistry;
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * <p>Note that the Venom 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.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVenom extends PWMSpeedController {
+  /**
+   * Constructor for a Venom connected via PWM.
+   *
+   * @param channel The PWM channel that the Venom is attached to. 0-9 are on-board, 10-19 are
+   *                on the MXP port
+   */
+  public PWMVenom(final int channel) {
+    super(channel);
+
+    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_FusionVenom, getChannel() + 1);
+    SendableRegistry.setName(this, "PWMVenom", 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..b51686a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM control.
+ *
+ * <p>Note that the Victor SPX 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 Victor SPX User
+ * Manual available from CTRE.
+ *
+ * <p><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVictorSPX extends PWMSpeedController {
+  /**
+   * Constructor for a Victor SPX connected via PWM.
+   *
+   * @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, 0.997);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel() + 1);
+    SendableRegistry.setName(this, "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..e30be1b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the Power Distribution
+ * Panel over CAN.
+ */
+public class PowerDistributionPanel implements Sendable, AutoCloseable {
+  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 + 1);
+    SendableRegistry.addLW(this, "PowerDistributionPanel", module);
+  }
+
+  /**
+   * Constructor.  Uses the default CAN ID (0).
+   */
+  public PowerDistributionPanel() {
+    this(0);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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..b25d907
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
@@ -0,0 +1,258 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.Collection;
+
+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 edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 preferences keys.
+   * @return a collection of the keys
+   */
+  public Collection<String> getKeys() {
+    return 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) {
+    requireNonNullParam(value, "value", "putString");
+
+    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..fde4ef1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -0,0 +1,315 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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;
+
+  /**
+   * 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 + 1);
+    }
+    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);
+
+    SendableRegistry.addLW(this, "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_channel = channel;
+    m_direction = requireNonNullParam(direction, "direction", "Relay");
+    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() {
+    SendableRegistry.remove(this);
+    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;
+  }
+
+  /**
+   * 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) {
+    requireNonNullParam(direction, "direction", "setDirection");
+    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..e2861f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.concurrent.locks.ReentrantLock;
+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
+  private static long m_threadId = -1;
+
+  private static void setupCameraServerShared() {
+    CameraServerShared shared = new CameraServerShared() {
+
+      @Override
+      public void reportVideoServer(int id) {
+        HAL.report(tResourceType.kResourceType_PCVideoServer, id + 1);
+      }
+
+      @Override
+      public void reportUsbCamera(int id) {
+        HAL.report(tResourceType.kResourceType_UsbCamera, id + 1);
+      }
+
+      @Override
+      public void reportDriverStationError(String error) {
+        DriverStation.reportError(error, true);
+      }
+
+      @Override
+      public void reportAxisCamera(int id) {
+        HAL.report(tResourceType.kResourceType_AxisCamera, id + 1);
+      }
+
+      @Override
+      public Long getRobotMainThreadId() {
+        return RobotBase.getMainThreadId();
+      }
+
+      @Override
+      public boolean isRoboRIO() {
+        return RobotBase.isReal();
+      }
+    };
+
+    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();
+    m_threadId = Thread.currentThread().getId();
+    setupCameraServerShared();
+    inst.setNetworkIdentity("Robot");
+    if (isReal()) {
+      inst.startServer("/home/lvuser/networktables.ini");
+    } else {
+      inst.startServer();
+    }
+    m_ds = DriverStation.getInstance();
+    inst.getTable("LiveWindow").getSubTable(".status").getEntry("LW Enabled").setBoolean(false);
+
+    LiveWindow.setEnabled(false);
+    Shuffleboard.disableActuatorWidgets();
+  }
+
+  public static long getMainThreadId() {
+    return m_threadId;
+  }
+
+  @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();
+
+  /**
+   * Ends the main loop in startCompetition().
+   */
+  public abstract void endCompetition();
+
+  @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);
+    }
+  }
+
+  private static final ReentrantLock m_runMutex = new ReentrantLock();
+  private static RobotBase m_robotCopy;
+  private static boolean m_suppressExitWarning;
+
+  /**
+   * Run the robot main loop.
+   */
+  @SuppressWarnings({"PMD.AvoidInstantiatingObjectsInLoops", "PMD.AvoidCatchingThrowable",
+                     "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+  private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
+    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);
+      return;
+    }
+
+    m_runMutex.lock();
+    m_robotCopy = robot;
+    m_runMutex.unlock();
+
+    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 {
+      m_runMutex.lock();
+      boolean suppressExitWarning = m_suppressExitWarning;
+      m_runMutex.unlock();
+      if (!suppressExitWarning) {
+        // 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);
+        }
+      }
+    }
+  }
+
+  /**
+   * Suppress the "Robots should not quit" message.
+   */
+  public static void suppressExitWarning(boolean value) {
+    m_runMutex.lock();
+    m_suppressExitWarning = value;
+    m_runMutex.unlock();
+  }
+
+  /**
+   * Starting point for the applications.
+   */
+  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, 0,
+        WPILibVersion.Version);
+
+    if (HAL.hasMain()) {
+      Thread thread = new Thread(() -> {
+        runRobot(robotSupplier);
+        HAL.exitMain();
+      }, "robot main");
+      thread.setDaemon(true);
+      thread.start();
+      HAL.runMain();
+      suppressExitWarning(true);
+      m_runMutex.lock();
+      RobotBase robot = m_robotCopy;
+      m_runMutex.unlock();
+      if (robot != null) {
+        robot.endCompetition();
+      }
+      try {
+        thread.join(1000);
+      } catch (InterruptedException ex) {
+        Thread.currentThread().interrupt();
+      }
+    } else {
+      runRobot(robotSupplier);
+    }
+
+    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..104fb26
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
@@ -0,0 +1,716 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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) {
+    requireNonNullParam(leftMotor, "leftMotor", "RobotDrive");
+    requireNonNullParam(rightMotor, "rightMotor", "RobotDrive");
+
+    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 = requireNonNullParam(frontLeftMotor, "frontLeftMotor", "RobotDrive");
+    m_rearLeftMotor = requireNonNullParam(rearLeftMotor, "rearLeftMotor", "RobotDrive");
+    m_frontRightMotor = requireNonNullParam(frontRightMotor, "frontRightMotor", "RobotDrive");
+    m_rearRightMotor = requireNonNullParam(rearRightMotor, "rearRightMotor", "RobotDrive");
+    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, tInstances.kRobotDrive_ArcadeRatioCurve,
+          getNumMotors());
+      kArcadeRatioCurve_Reported = true;
+    }
+    if (curve < 0) {
+      double value = Math.log(-curve);
+      double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+      if (ratio == 0) {
+        ratio = 0.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 = 0.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) {
+    requireNonNullParam(leftStick, "leftStick", "tankDrive");
+    requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+    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) {
+    requireNonNullParam(leftStick, "leftStick", "tankDrive");
+    requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+    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) {
+    requireNonNullParam(leftStick, "leftStick", "tankDrive");
+    requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+    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) {
+    requireNonNullParam(leftStick, "leftStick", "tankDrive");
+    requireNonNullParam(rightStick, "rightStick", "tankDrive");
+
+    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, tInstances.kRobotDrive_Tank,
+          getNumMotors());
+      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, tInstances.kRobotDrive_ArcadeStandard,
+          getNumMotors());
+      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, tInstances.kRobotDrive_MecanumCartesian,
+          getNumMotors());
+      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, tInstances.kRobotDrive_MecanumPolar,
+          getNumMotors());
+      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) {
+
+    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;
+  }
+
+  /**
+   * 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..58004d5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 isEStopped() {
+    return DriverStation.getInstance().isEStopped();
+  }
+
+  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..b2e98b6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * Mindsensors SD540 Speed Controller.
+ *
+ * <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><ul>
+ * <li>2.05ms = full "forward"
+ * <li>1.55ms = the "high end" of the deadband range
+ * <li>1.50ms = center of the deadband range (off)
+ * <li>1.44ms = the "low end" of the deadband range
+ * <li>0.94ms = full "reverse"
+ * </ul>
+ */
+public class SD540 extends PWMSpeedController {
+  /**
+   * Common initialization code called by all constructors.
+   */
+  protected void initSD540() {
+    setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
+    SendableRegistry.setName(this, "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..aca2cbe
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -0,0 +1,806 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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 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;
+
+    SPIJNI.spiInitialize(m_port);
+
+    HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
+  }
+
+  @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);
+  }
+
+  /**
+   * Configure the Auto SPI Stall time between reads.
+   *
+   * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
+   * @param stallTicks the number of ticks to stall for
+   * @param pow2BytesPerRead the number of bytes to read before stalling
+   */
+  public void configureAutoStall(int csToSclkTicks, int stallTicks, int pow2BytesPerRead) {
+    SPIJNI.spiConfigureAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead);
+  }
+
+  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/Sendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
new file mode 100644
index 0000000..138f7f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+
+/**
+ * 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
+   * @deprecated Use SendableRegistry.getName()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default String getName() {
+    return SendableRegistry.getName(this);
+  }
+
+  /**
+   * Sets the name of this {@link Sendable} object.
+   *
+   * @param name name
+   * @deprecated Use SendableRegistry.setName()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default void setName(String name) {
+    SendableRegistry.setName(this, name);
+  }
+
+  /**
+   * Sets both the subsystem name and device name of this {@link Sendable} object.
+   *
+   * @param subsystem subsystem name
+   * @param name device name
+   * @deprecated Use SendableRegistry.setName()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default void setName(String subsystem, String name) {
+    SendableRegistry.setName(this, subsystem, 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
+   * @deprecated Use SendableRegistry.setName()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default void setName(String moduleType, int channel) {
+    SendableRegistry.setName(this, 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)
+   * @deprecated Use SendableRegistry.setName()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default void setName(String moduleType, int moduleNumber, int channel) {
+    SendableRegistry.setName(this, moduleType, moduleNumber, channel);
+  }
+
+  /**
+   * Gets the subsystem name of this {@link Sendable} object.
+   *
+   * @return Subsystem name
+   * @deprecated Use SendableRegistry.getSubsystem()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default String getSubsystem() {
+    return SendableRegistry.getSubsystem(this);
+  }
+
+  /**
+   * Sets the subsystem name of this {@link Sendable} object.
+   *
+   * @param subsystem subsystem name
+   * @deprecated Use SendableRegistry.setSubsystem()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default void setSubsystem(String subsystem) {
+    SendableRegistry.setSubsystem(this, subsystem);
+  }
+
+  /**
+   * Add a child component.
+   *
+   * @param child child component
+   * @deprecated Use SendableRegistry.addChild()
+   */
+  @Deprecated(since = "2020", forRemoval = true)
+  default void addChild(Object child) {
+    SendableRegistry.addChild(this, child);
+  }
+
+  /**
+   * 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..4e2116c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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.SendableRegistry;
+
+/**
+ * Base class for all sensors. Stores most recent status information as well as containing utility
+ * functions for checking channels and error processing.
+ * @deprecated Use Sendable and SendableRegistry
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public abstract class SendableBase implements Sendable, AutoCloseable {
+  /**
+   * 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) {
+      SendableRegistry.addLW(this, "");
+    } else {
+      SendableRegistry.add(this, "");
+    }
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+}
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..86bd941
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -0,0 +1,378 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.charset.StandardCharsets;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SerialPortJNI;
+
+/**
+ * Driver for the serial ports (USB, MXP, Onboard) on the roboRIO.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class SerialPort implements AutoCloseable {
+  private int m_portHandle;
+
+  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 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.
+   *
+   * <p>Prefer to use the constructor that doesn't take a port name, but in some
+   * cases the automatic detection might not work correctly.
+   *
+   * @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
+   */
+  public SerialPort(final int baudRate, String portName, Port port, final int dataBits,
+                    Parity parity, StopBits stopBits) {
+    m_portHandle = SerialPortJNI.serialInitializePortDirect((byte) port.value, portName);
+    SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
+    SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
+    SerialPortJNI.serialSetParity(m_portHandle, (byte) parity.value);
+    SerialPortJNI.serialSetStopBits(m_portHandle, (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, port.value + 1);
+  }
+
+  /**
+   * 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_portHandle = SerialPortJNI.serialInitializePort((byte) port.value);
+    SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
+    SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
+    SerialPortJNI.serialSetParity(m_portHandle, (byte) parity.value);
+    SerialPortJNI.serialSetStopBits(m_portHandle, (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, port.value + 1);
+  }
+
+  /**
+   * 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_portHandle);
+  }
+
+  /**
+   * 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_portHandle, (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_portHandle, 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_portHandle);
+  }
+
+  /**
+   * 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_portHandle);
+  }
+
+  /**
+   * 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);
+    return new String(out, 0, out.length, StandardCharsets.US_ASCII);
+  }
+
+  /**
+   * 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_portHandle, 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_portHandle, 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_portHandle, 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_portHandle, 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_portHandle, 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_portHandle, (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_portHandle);
+  }
+
+  /**
+   * 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_portHandle);
+  }
+}
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..6b2eab6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 = 0.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() + 1);
+    SendableRegistry.setName(this, "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/SlewRateLimiter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
new file mode 100644
index 0000000..2ba468b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.wpiutil.math.MathUtil;
+
+/**
+ * A class that limits the rate of change of an input value.  Useful for implementing voltage,
+ * setpoint, and/or output ramps.  A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a
+ * {@link edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
+ */
+public class SlewRateLimiter {
+  private final Timer m_timer = new Timer();
+  private final double m_rateLimit;
+  private double m_prevVal;
+
+  /**
+   * Creates a new SlewRateLimiter with the given rate limit and initial value.
+   *
+   * @param rateLimit The rate-of-change limit, in units per second.
+   * @param initialValue The initial value of the input.
+   */
+  public SlewRateLimiter(double rateLimit, double initialValue) {
+    m_prevVal = initialValue;
+    m_rateLimit = rateLimit;
+    m_timer.start();
+  }
+
+  /**
+   * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
+   *
+   * @param rateLimit The rate-of-change limit, in units per second.
+   */
+  public SlewRateLimiter(double rateLimit) {
+    this(rateLimit, 0);
+  }
+
+  /**
+   * Filters the input to limit its slew rate.
+   *
+   * @param input The input value whose slew rate is to be limited.
+   * @return The filtered value, which will not change faster than the slew rate.
+   */
+  public double calculate(double input) {
+    m_prevVal += MathUtil.clamp(input - m_prevVal,
+                                -m_rateLimit * m_timer.get(),
+                                m_rateLimit * m_timer.get());
+    m_timer.reset();
+    return m_prevVal;
+  }
+
+  /**
+   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+   *
+   * @param value The value to reset to.
+   */
+  public void reset(double value) {
+    m_timer.reset();
+    m_prevVal = value;
+  }
+}
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..163415c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  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 + 1, m_moduleNumber + 1);
+    SendableRegistry.addLW(this, "Solenoid", m_moduleNumber, m_channel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    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..a778d8c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 class SolenoidBase {
+  protected final int m_moduleNumber; // The number of the solenoid module being used.
+
+  /**
+   * Constructor.
+   *
+   * @param moduleNumber The PCM CAN ID
+   */
+  protected 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..c8bacbf
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * REV Robotics SPARK Speed Controller.
+ *
+ * <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><ul>
+ * <li>2.003ms = full "forward"
+ * <li>1.550ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.460ms = the "low end" of the deadband range
+ * <li>0.999ms = full "reverse"
+ * </ul>
+ */
+public class Spark extends PWMSpeedController {
+  /**
+   * Common initialization code called by all constructors.
+   */
+  protected void initSpark() {
+    setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
+    SendableRegistry.setName(this, "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..4407ff0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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);
+
+  /**
+   * Sets the voltage output of the SpeedController.  Compensates for the current bus
+   * voltage to ensure that the desired voltage is output even if the battery voltage is below
+   * 12V - highly useful when the voltage outputs are "meaningful" (e.g. they come from a
+   * feedforward calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
+   * properly - unlike the ordinary set function, it is not "set it and forget it."
+   *
+   * @param outputVolts The voltage to output.
+   */
+  default void setVoltage(double outputVolts) {
+    set(outputVolts / RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * 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..7dd3f76
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * Allows multiple {@link SpeedController} objects to be linked together.
+ */
+public class SpeedControllerGroup implements SpeedController, Sendable, AutoCloseable {
+  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;
+    SendableRegistry.addChild(this, speedController);
+    for (int i = 0; i < speedControllers.length; i++) {
+      m_speedControllers[i + 1] = speedControllers[i];
+      SendableRegistry.addChild(this, speedControllers[i]);
+    }
+    instances++;
+    SendableRegistry.addLW(this, "tSpeedControllerGroup", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  @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..6f20869
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
+ *
+ * <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><ul>
+ * <li>2.037ms = full "forward"
+ * <li>1.539ms = the "high end" of the deadband range
+ * <li>1.513ms = center of the deadband range (off)
+ * <li>1.487ms = the "low end" of the deadband range
+ * <li>0.989ms = full "reverse"
+ * </ul>
+ */
+public class Talon extends PWMSpeedController {
+  /**
+   * Constructor for a Talon (original or Talon SR).
+   *
+   * @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, 0.989);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_Talon, getChannel() + 1);
+    SendableRegistry.setName(this, "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..b6b6b25
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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);
+    NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
+
+    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();
+    }
+  }
+
+  /**
+   * Ends the main loop in startCompetition().
+   */
+  @Override
+  public void endCompetition() {
+    NotifierJNI.stopNotifier(m_notifier);
+  }
+
+  /**
+   * 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..e1e312f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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() {
+    m_accumulatedTime = get();
+    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..db7454a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -0,0 +1,412 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+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 implements PIDSource, Sendable, AutoCloseable {
+  /**
+   * 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 volatile 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;
+
+  private SimDevice m_simDevice;
+  private SimBoolean m_simRangeValid;
+  private SimDouble m_simRange;
+
+  /**
+   * 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() {
+      while (m_automaticEnabled) {
+        for (Ultrasonic sensor: m_sensors) {
+          if (!m_automaticEnabled) {
+            break;
+          }
+
+          if (sensor.isEnabled()) {
+            sensor.m_pingChannel.pulse(kPingTime);  // do the ping
+          }
+
+          Timer.delay(0.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() {
+    m_simDevice = SimDevice.create("Ultrasonic", m_echoChannel.getChannel());
+    if (m_simDevice != null) {
+      m_simRangeValid = m_simDevice.createBoolean("Range Valid", false, true);
+      m_simRange = m_simDevice.createDouble("Range (in)", false, 0.0);
+      m_pingChannel.setSimDevice(m_simDevice);
+      m_echoChannel.setSimDevice(m_simDevice);
+    }
+    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
+    SendableRegistry.addChild(this, 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);
+    SendableRegistry.addLW(this, "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);
+    SendableRegistry.addChild(this, m_pingChannel);
+    SendableRegistry.addChild(this, 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() {
+    SendableRegistry.remove(this);
+    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);
+    }
+
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
+  }
+
+  /**
+   * 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() {
+    if (m_simRangeValid != null) {
+      return m_simRangeValid.get();
+    }
+    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()) {
+      if (m_simRange != null) {
+        return m_simRange.get();
+      }
+      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/Victor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
new file mode 100644
index 0000000..2d19240
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * 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.
+ *
+ * <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><ul>
+ * <li>2.027ms = full "forward"
+ * <li>1.525ms = the "high end" of the deadband range
+ * <li>1.507ms = center of the deadband range (off)
+ * <li>1.490ms = the "low end" of the deadband range
+ * <li>1.026ms = full "reverse"
+ * </ul>
+ */
+public class Victor extends PWMSpeedController {
+  /**
+   * Constructor.
+   *
+   * @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() + 1);
+    SendableRegistry.setName(this, "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..22b3513
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.SendableRegistry;
+
+/**
+ * VEX Robotics Victor SP Speed Controller.
+ *
+ * <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><ul>
+ * <li>2.004ms = full "forward"
+ * <li>1.520ms = the "high end" of the deadband range
+ * <li>1.500ms = center of the deadband range (off)
+ * <li>1.480ms = the "low end" of the deadband range
+ * <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class VictorSP extends PWMSpeedController {
+  /**
+   * Constructor.
+   *
+   * @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, 0.997);
+    setPeriodMultiplier(PeriodMultiplier.k1X);
+    setSpeed(0.0);
+    setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_VictorSP, getChannel() + 1);
+    SendableRegistry.setName(this, "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..223e992
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
@@ -0,0 +1,283 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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(Watchdog::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.
+    return Long.compare(m_expirationTime, rhs.m_expirationTime);
+  }
+
+  /**
+   * 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_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;
+  }
+
+
+  @SuppressWarnings("PMD.AvoidDeeplyNestedIfStmts")
+  private static void schedulerFunc() {
+    m_queueMutex.lock();
+
+    try {
+      while (!Thread.currentThread().isInterrupted()) {
+        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..fd6161a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -0,0 +1,345 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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.
+   */
+  public 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"})
+    public 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 + 1);
+  }
+
+  /**
+   * 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/controller/ArmFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
new file mode 100644
index 0000000..33f9784
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as a motor
+ * acting against the force of gravity on a beam suspended at an angle).
+ */
+@SuppressWarnings("MemberName")
+public class ArmFeedforward {
+  public final double ks;
+  public final double kcos;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.  Units of the gain values
+   * will dictate units of the computed feedforward.
+   *
+   * @param ks   The static gain.
+   * @param kcos The gravity gain.
+   * @param kv   The velocity gain.
+   * @param ka   The acceleration gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+    this.ks = ks;
+    this.kcos = kcos;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.  Acceleration gain is
+   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks   The static gain.
+   * @param kcos The gravity gain.
+   * @param kv   The velocity gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv) {
+    this(ks, kcos, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param positionRadians       The position (angle) setpoint.
+   * @param velocityRadPerSec     The velocity setpoint.
+   * @param accelRadPerSecSquared The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double positionRadians, double velocityRadPerSec,
+                          double accelRadPerSecSquared) {
+    return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
+        + kv * velocityRadPerSec
+        + ka * accelRadPerSecSquared;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+   * be zero).
+   *
+   * @param positionRadians The position (angle) setpoint.
+   * @param velocity        The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double positionRadians, double velocity) {
+    return calculate(positionRadians, velocity, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param acceleration The acceleration of the arm.
+   * @return The maximum possible velocity at the given acceleration and angle.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param acceleration The acceleration of the arm.
+   * @return The minimum possible velocity at the given acceleration and angle.
+   */
+  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply, a position, and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param velocity The velocity of the arm.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply, a position, and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param velocity The velocity of the arm.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
new file mode 100644
index 0000000..86a4f7e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
+ * acting against the force of gravity).
+ */
+@SuppressWarnings("MemberName")
+public class ElevatorFeedforward {
+  public final double ks;
+  public final double kg;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.  Units of the gain values
+   * will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+    this.ks = ks;
+    this.kg = kg;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.  Acceleration gain is
+   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+   * be zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - kg - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
new file mode 100644
index 0000000..eea0644
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -0,0 +1,384 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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.controller;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Implements a PID control loop.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class PIDController implements Sendable, AutoCloseable {
+  private static int instances;
+
+  // Factor for "proportional" control
+  @SuppressWarnings("MemberName")
+  private double m_Kp;
+
+  // Factor for "integral" control
+  @SuppressWarnings("MemberName")
+  private double m_Ki;
+
+  // Factor for "derivative" control
+  @SuppressWarnings("MemberName")
+  private double m_Kd;
+
+  // The period (in seconds) of the loop that calls the controller
+  private final double m_period;
+
+  private double m_maximumIntegral = 1.0;
+
+  private double m_minimumIntegral = -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? eg. Absolute encoder
+  private boolean m_continuous;
+
+  // The error at the time of the most recent call to calculate()
+  private double m_positionError;
+  private double m_velocityError;
+
+  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
+  private double m_prevError;
+
+  // The sum of the errors for use in the integral calc
+  private double m_totalError;
+
+  // The percentage or absolute error that is considered at setpoint.
+  private double m_positionTolerance = 0.05;
+  private double m_velocityTolerance = Double.POSITIVE_INFINITY;
+
+  private double m_setpoint;
+
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
+   * 0.02 seconds.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(double Kp, double Ki, double Kd) {
+    this(Kp, Ki, Kd, 0.02);
+  }
+
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp     The proportional coefficient.
+   * @param Ki     The integral coefficient.
+   * @param Kd     The derivative coefficient.
+   * @param period The period between controller updates in seconds.
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(double Kp, double Ki, double Kd, double period) {
+    m_Kp = Kp;
+    m_Ki = Ki;
+    m_Kd = Kd;
+
+    m_period = period;
+
+    instances++;
+    SendableRegistry.addLW(this, "PIDController", instances);
+
+    HAL.report(tResourceType.kResourceType_PIDController2, instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Set the proportional, integral, and differential coefficients.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   */
+  @SuppressWarnings("ParameterName")
+  public void setPID(double Kp, double Ki, double Kd) {
+    m_Kp = Kp;
+    m_Ki = Ki;
+    m_Kd = Kd;
+  }
+
+  /**
+   * Sets the Proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setP(double Kp) {
+    m_Kp = Kp;
+  }
+
+  /**
+   * Sets the Integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setI(double Ki) {
+    m_Ki = Ki;
+  }
+
+  /**
+   * Sets the Differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setD(double Kd) {
+    m_Kd = Kd;
+  }
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_Kp;
+  }
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_Ki;
+  }
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_Kd;
+  }
+
+  /**
+   * Returns the period of this controller.
+   *
+   * @return the period of the controller.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  public void setSetpoint(double setpoint) {
+    if (m_maximumInput > m_minimumInput) {
+      m_setpoint = MathUtil.clamp(setpoint, m_minimumInput, m_maximumInput);
+    } else {
+      m_setpoint = setpoint;
+    }
+  }
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the percentage of the total input range, determined by
+   * SetTolerance. This asssumes that the maximum and minimum input were set using SetInput.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  public boolean atSetpoint() {
+    return Math.abs(m_positionError) < m_positionTolerance
+        && Math.abs(m_velocityError) < m_velocityTolerance;
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>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 minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_continuous = true;
+    setInputRange(minimumInput, maximumInput);
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  public void disableContinuousInput() {
+    m_continuous = false;
+  }
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * <p>When the cap is reached, the integrator value is added to the controller
+   * output rather than the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_minimumIntegral = minimumIntegral;
+    m_maximumIntegral = maximumIntegral;
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance) {
+    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance, double velocityTolerance) {
+    m_positionTolerance = positionTolerance;
+    m_velocityTolerance = velocityTolerance;
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getPositionError() {
+    return getContinuousError(m_positionError);
+  }
+
+  /**
+   * Returns the velocity error.
+   */
+  public double getVelocityError() {
+    return m_velocityError;
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint    The new setpoint of the controller.
+   */
+  public double calculate(double measurement, double setpoint) {
+    // Set setpoint to provided value
+    setSetpoint(setpoint);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  public double calculate(double measurement) {
+    m_prevError = m_positionError;
+    m_positionError = getContinuousError(m_setpoint - measurement);
+    m_velocityError = (m_positionError - m_prevError) / m_period;
+
+    if (m_Ki != 0) {
+      m_totalError = MathUtil.clamp(m_totalError + m_positionError * m_period,
+          m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+    }
+
+    return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+  }
+
+  /**
+   * Resets the previous error and the integral term.
+   */
+  public void reset() {
+    m_prevError = 0;
+    m_totalError = 0;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+  }
+
+  /**
+   * Wraps error around for continuous inputs. The original error is returned if continuous mode is
+   * disabled.
+   *
+   * @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;
+  }
+
+  /**
+   * Sets the minimum and maximum values expected from the input.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  private void setInputRange(double minimumInput, double maximumInput) {
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+    m_inputRange = maximumInput - minimumInput;
+
+    // Clamp setpoint to new input
+    if (m_maximumInput > m_minimumInput) {
+      m_setpoint = MathUtil.clamp(m_setpoint, m_minimumInput, m_maximumInput);
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
new file mode 100644
index 0000000..7550aa5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
@@ -0,0 +1,360 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.controller;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class ProfiledPIDController implements Sendable {
+  private static int instances;
+
+  private PIDController m_controller;
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+  private TrapezoidProfile.Constraints m_constraints;
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+   * Kd.
+   *
+   * @param Kp          The proportional coefficient.
+   * @param Ki          The integral coefficient.
+   * @param Kd          The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  @SuppressWarnings("ParameterName")
+  public ProfiledPIDController(double Kp, double Ki, double Kd,
+                        TrapezoidProfile.Constraints constraints) {
+    this(Kp, Ki, Kd, constraints, 0.02);
+  }
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+   * Kd.
+   *
+   * @param Kp          The proportional coefficient.
+   * @param Ki          The integral coefficient.
+   * @param Kd          The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @param period      The period between controller updates in seconds. The
+   *                    default is 0.02 seconds.
+   */
+  @SuppressWarnings("ParameterName")
+  public ProfiledPIDController(double Kp, double Ki, double Kd,
+                        TrapezoidProfile.Constraints constraints,
+                        double period) {
+    m_controller = new PIDController(Kp, Ki, Kd, period);
+    m_constraints = constraints;
+    instances++;
+    HAL.report(tResourceType.kResourceType_ProfiledPIDController, instances);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setPID(double Kp, double Ki, double Kd) {
+    m_controller.setPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setP(double Kp) {
+    m_controller.setP(Kp);
+  }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setI(double Ki) {
+    m_controller.setI(Ki);
+  }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setD(double Kd) {
+    m_controller.setD(Kd);
+  }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_controller.getP();
+  }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_controller.getI();
+  }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_controller.getD();
+  }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  public double getPeriod() {
+    return m_controller.getPeriod();
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal state.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal position.
+   */
+  public void setGoal(double goal) {
+    m_goal = new TrapezoidProfile.State(goal, 0);
+  }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   */
+  public TrapezoidProfile.State getGoal() {
+    return m_goal;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   */
+  public boolean atGoal() {
+    return atSetpoint() && m_goal.equals(m_setpoint);
+  }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  public void setConstraints(TrapezoidProfile.Constraints constraints) {
+    m_constraints = constraints;
+  }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  public TrapezoidProfile.State getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   */
+  public boolean atSetpoint() {
+    return m_controller.atSetpoint();
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>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 minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_controller.enableContinuousInput(minimumInput, maximumInput);
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  public void disableContinuousInput() {
+    m_controller.disableContinuousInput();
+  }
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * <p>When the cap is reached, the integrator value is added to the controller
+   * output rather than the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance) {
+    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance, double velocityTolerance) {
+    m_controller.setTolerance(positionTolerance, velocityTolerance);
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getPositionError() {
+    return m_controller.getPositionError();
+  }
+
+  /**
+   * Returns the change in error per second.
+   */
+  public double getVelocityError() {
+    return m_controller.getVelocityError();
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  public double calculate(double measurement) {
+    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
+    m_setpoint = profile.calculate(getPeriod());
+    return m_controller.calculate(measurement, m_setpoint.position);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  public double calculate(double measurement, TrapezoidProfile.State goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PIDController.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  public double calculate(double measurement, double goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal        The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  public double calculate(double measurement, TrapezoidProfile.State goal,
+                   TrapezoidProfile.Constraints constraints) {
+    setConstraints(constraints);
+    return calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
+   */
+  public void reset(TrapezoidProfile.State measurement) {
+    m_controller.reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  public void reset(double measuredPosition, double measuredVelocity) {
+    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The velocity is
+   *     assumed to be zero.
+   */
+  public void reset(double measuredPosition) {
+    reset(measuredPosition, 0.0);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("ProfiledPIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
new file mode 100644
index 0000000..3ff9ac5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.controller;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models
+ * that drives the model to a desired pose along a two-dimensional trajectory.
+ * Why would we need a nonlinear control law in addition to the linear ones we
+ * have used so far like PID? If we use the original approach with PID
+ * controllers for left and right position and velocity states, the controllers
+ * only deal with the local pose. If the robot deviates from the path, there is
+ * no way for the controllers to correct and the robot may not reach the desired
+ * global pose. This is due to multiple endpoints existing for the robot which
+ * have the same encoder path arc lengths.
+ *
+ * <p>Instead of using wheel path arc lengths (which are in the robot's local
+ * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
+ * global pose. The controller uses this extra information to guide a linear
+ * reference tracker like the PID controllers back in by adjusting the
+ * references of the PID controllers.
+ *
+ * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
+ * describes a nonlinear controller for a wheeled vehicle with unicycle-like
+ * kinematics; a global pose consisting of x, y, and theta; and a desired pose
+ * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
+ * acronym for the title of the book it came from in Italian ("Robotica
+ * Articolata e Mobile per i SErvizi e le TEcnologie").
+ *
+ * <p>See
+ * <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">
+ * Controls Engineering in the FIRST Robotics Competition</a> section on Ramsete
+ * unicycle controller for a derivation and analysis.
+ */
+public class RamseteController {
+  @SuppressWarnings("MemberName")
+  private final double m_b;
+  @SuppressWarnings("MemberName")
+  private final double m_zeta;
+
+  private Pose2d m_poseError = new Pose2d();
+  private Pose2d m_poseTolerance = new Pose2d();
+
+  /**
+   * Construct a Ramsete unicycle controller.
+   *
+   * @param b    Tuning parameter (b &gt; 0) for which larger values make convergence more
+   *             aggressive like a proportional term.
+   * @param zeta Tuning parameter (0 &lt; zeta &lt; 1) for which larger values provide more damping
+   *             in response.
+   */
+  @SuppressWarnings("ParameterName")
+  public RamseteController(double b, double zeta) {
+    m_b = b;
+    m_zeta = zeta;
+  }
+
+  /**
+   * Construct a Ramsete unicycle controller. The default arguments for
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+   * results.
+   */
+  public RamseteController() {
+    this(2.0, 0.7);
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    final var eTranslate = m_poseError.getTranslation();
+    final var eRotate = m_poseError.getRotation();
+    final var tolTranslate = m_poseTolerance.getTranslation();
+    final var tolRotate = m_poseTolerance.getRotation();
+    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+           && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+           && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * atReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  public void setTolerance(Pose2d poseTolerance) {
+    m_poseTolerance = poseTolerance;
+  }
+
+  /**
+   * Returns the next output of the Ramsete controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come
+   * from a drivetrain trajectory.
+   *
+   * @param currentPose                        The current pose.
+   * @param poseRef                            The desired pose.
+   * @param linearVelocityRefMeters            The desired linear velocity in meters.
+   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in meters.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(Pose2d currentPose,
+                                 Pose2d poseRef,
+                                 double linearVelocityRefMeters,
+                                 double angularVelocityRefRadiansPerSecond) {
+    m_poseError = poseRef.relativeTo(currentPose);
+
+    // Aliases for equation readability
+    final double eX = m_poseError.getTranslation().getX();
+    final double eY = m_poseError.getTranslation().getY();
+    final double eTheta = m_poseError.getRotation().getRadians();
+    final double vRef = linearVelocityRefMeters;
+    final double omegaRef = angularVelocityRefRadiansPerSecond;
+
+    double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
+
+    return new ChassisSpeeds(vRef * m_poseError.getRotation().getCos() + k * eX,
+                             0.0,
+                             omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
+  }
+
+  /**
+   * Returns the next output of the Ramsete controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come
+   * from a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
+    return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond,
+        desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
+  }
+
+  /**
+   * Returns sin(x) / x.
+   *
+   * @param x Value of which to take sinc(x).
+   */
+  @SuppressWarnings("ParameterName")
+  private static double sinc(double x) {
+    if (Math.abs(x) < 1e-9) {
+      return 1.0 - 1.0 / 6.0 * x * x;
+    } else {
+      return Math.sin(x) / x;
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
new file mode 100644
index 0000000..9d2c57e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
+ */
+@SuppressWarnings("MemberName")
+public class SimpleMotorFeedforward {
+  public final double ks;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.  Units of the gain values
+   * will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv, double ka) {
+    this.ks = ks;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.  Acceleration gain is
+   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv) {
+    this(ks, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+   * be zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}
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..cbfd3c0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -0,0 +1,436 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.Sendable;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * 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 {
+ *   SpeedController m_frontLeft = new PWMVictorSPX(1);
+ *   SpeedController m_rearLeft = new PWMVictorSPX(2);
+ *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
+ *
+ *   SpeedController m_frontRight = new PWMVictorSPX(3);
+ *   SpeedController m_rearRight = new PWMVictorSPX(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 {
+ *   SpeedController m_frontLeft = new PWMVictorSPX(1);
+ *   SpeedController m_midLeft = new PWMVictorSPX(2);
+ *   SpeedController m_rearLeft = new PWMVictorSPX(3);
+ *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
+ *
+ *   SpeedController m_frontRight = new PWMVictorSPX(4);
+ *   SpeedController m_midRight = new PWMVictorSPX(5);
+ *   SpeedController m_rearRight = new PWMVictorSPX(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 implements Sendable, AutoCloseable {
+  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;
+    SendableRegistry.addChild(this, m_leftMotor);
+    SendableRegistry.addChild(this, m_rightMotor);
+    instances++;
+    SendableRegistry.addLW(this, "DifferentialDrive", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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,
+                 tInstances.kRobotDrive2_DifferentialArcade, 2);
+      m_reported = true;
+    }
+
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    xSpeed = applyDeadband(xSpeed, m_deadband);
+
+    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+    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(MathUtil.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
+    double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
+    m_rightMotor.set(MathUtil.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+
+    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,
+                 tInstances.kRobotDrive2_DifferentialCurvature, 2);
+      m_reported = true;
+    }
+
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    xSpeed = applyDeadband(xSpeed, m_deadband);
+
+    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+    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 * MathUtil.clamp(zRotation, -1.0, 1.0) * 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,
+                 tInstances.kRobotDrive2_DifferentialTank, 2);
+      m_reported = true;
+    }
+
+    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
+    leftSpeed = applyDeadband(leftSpeed, m_deadband);
+
+    rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
+    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..45da78a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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.Sendable;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  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)));
+    SendableRegistry.addChild(this, m_leftMotor);
+    SendableRegistry.addChild(this, m_rightMotor);
+    SendableRegistry.addChild(this, m_backMotor);
+    instances++;
+    SendableRegistry.addLW(this, "KilloughDrive", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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,
+                 tInstances.kRobotDrive2_KilloughCartesian, 3);
+      m_reported = true;
+    }
+
+    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+    ySpeed = applyDeadband(ySpeed, m_deadband);
+
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    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,
+                 tInstances.kRobotDrive2_KilloughPolar, 3);
+      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..702bdf5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -0,0 +1,267 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.Sendable;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * 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 implements Sendable, AutoCloseable {
+  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;
+    SendableRegistry.addChild(this, m_frontLeftMotor);
+    SendableRegistry.addChild(this, m_rearLeftMotor);
+    SendableRegistry.addChild(this, m_frontRightMotor);
+    SendableRegistry.addChild(this, m_rearRightMotor);
+    instances++;
+    SendableRegistry.addLW(this, "MecanumDrive", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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,
+                 tInstances.kRobotDrive2_MecanumCartesian, 4);
+      m_reported = true;
+    }
+
+    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+    ySpeed = applyDeadband(ySpeed, m_deadband);
+
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    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, tInstances.kRobotDrive2_MecanumPolar, 4);
+      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..785a0c5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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;
+
+/**
+ * Common base class for drive platforms.
+ */
+public abstract class RobotDriveBase extends MotorSafety {
+  public static final double kDefaultDeadband = 0.02;
+  public static final double kDefaultMaxOutput = 1.0;
+
+  protected double m_deadband = kDefaultDeadband;
+  protected double m_maxOutput = kDefaultMaxOutput;
+
+  /**
+   * 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() {
+    setSafetyEnabled(true);
+  }
+
+  /**
+   * 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();
+
+  /**
+   * 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..257d36a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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.
+ *
+ * @deprecated This class is no longer used.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+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..cbeb4aa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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.PIDSource;
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * 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!
+ *
+ * @deprecated Use LinearFilter class instead.
+ */
+@Deprecated
+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/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
new file mode 100644
index 0000000..1fa2c8d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
@@ -0,0 +1,271 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+@JsonSerialize(using = Pose2d.PoseSerializer.class)
+@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
+public class Pose2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   * (Translation2d{0, 0} and Rotation{0})
+   */
+  public Pose2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation    The rotational component of the pose.
+   */
+  public Pose2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Convenience constructors that takes in x and y values directly instead of
+   * having to construct a Translation2d.
+   *
+   * @param x        The x component of the translational component of the pose.
+   * @param y        The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  @SuppressWarnings("ParameterName")
+  public Pose2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * <p>The matrix multiplication is as follows
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [0,    0,   1][transform.t]
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d plus(Transform2d other) {
+    return transformBy(other);
+  }
+
+  /**
+   * Returns the Transform2d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  public Transform2d minus(Pose2d other) {
+    final var pose = this.relativeTo(other);
+    return new Transform2d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d transformBy(Transform2d other) {
+    return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+        m_rotation.plus(other.getRotation()));
+  }
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * <p>This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   *              the current pose will be converted into.
+   * @return The current pose relative to the new origin pose.
+   */
+  public Pose2d relativeTo(Pose2d other) {
+    var transform = new Transform2d(other, this);
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
+   * Controls Engineering in the FIRST Robotics Competition</a>
+   * section on nonlinear pose estimation for derivation.
+   *
+   * <p>The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs exp() on the previous known
+   * field-relative pose with the argument being the twist, the user will
+   * receive the new field-relative pose.
+   *
+   * <p>"Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the
+   *              previous pose update. For example, if a non-holonomic robot moves forward
+   *              0.01 meters and changes angle by 0.5 degrees since the previous pose update,
+   *              the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * @return The new pose of the robot.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d exp(Twist2d twist) {
+    double dx = twist.dx;
+    double dy = twist.dy;
+    double dtheta = twist.dtheta;
+
+    double sinTheta = Math.sin(dtheta);
+    double cosTheta = Math.cos(dtheta);
+
+    double s;
+    double c;
+    if (Math.abs(dtheta) < 1E-9) {
+      s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+      c = 0.5 * dtheta;
+    } else {
+      s = sinTheta / dtheta;
+      c = (1 - cosTheta) / dtheta;
+    }
+    var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
+        new Rotation2d(cosTheta, sinTheta));
+
+    return this.plus(transform);
+  }
+
+  /**
+   * Returns a Twist2d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   * @return The twist that maps this to end.
+   */
+  public Twist2d log(Pose2d end) {
+    final var transform = end.relativeTo(this);
+    final var dtheta = transform.getRotation().getRadians();
+    final var halfDtheta = dtheta / 2.0;
+
+    final var cosMinusOne = transform.getRotation().getCos() - 1;
+
+    double halfThetaByTanOfHalfDtheta;
+    if (Math.abs(cosMinusOne) < 1E-9) {
+      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+    } else {
+      halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
+    }
+
+    Translation2d translationPart = transform.getTranslation().rotateBy(
+        new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
+    ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Pose2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Pose2d) {
+      return ((Pose2d) obj).m_translation.equals(m_translation)
+          && ((Pose2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+
+  static class PoseSerializer extends StdSerializer<Pose2d> {
+    PoseSerializer() {
+      super(Pose2d.class);
+    }
+
+    @Override
+    public void serialize(
+            Pose2d value, JsonGenerator jgen, SerializerProvider provider)
+            throws IOException, JsonProcessingException {
+
+      jgen.writeStartObject();
+      jgen.writeObjectField("translation", value.m_translation);
+      jgen.writeObjectField("rotation", value.m_rotation);
+      jgen.writeEndObject();
+    }
+  }
+
+  static class PoseDeserializer extends StdDeserializer<Pose2d> {
+    PoseDeserializer() {
+      super(Pose2d.class);
+    }
+
+    @Override
+    public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
+            throws IOException, JsonProcessingException {
+      JsonNode node = jp.getCodec().readTree(jp);
+
+      Translation2d translation =
+              jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
+      Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
+      return new Pose2d(translation, rotation);
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
new file mode 100644
index 0000000..288fa5c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+@JsonSerialize(using = Rotation2d.RotationSerializer.class)
+@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
+public class Rotation2d {
+  private final double m_value;
+  private final double m_cos;
+  private final double m_sin;
+
+  /**
+   * Constructs a Rotation2d with a default angle of 0 degrees.
+   */
+  public Rotation2d() {
+    m_value = 0.0;
+    m_cos = 1.0;
+    m_sin = 0.0;
+  }
+
+  /**
+   * Constructs a Rotation2d with the given radian value.
+   * The x and y don't have to be normalized.
+   *
+   * @param value The value of the angle in radians.
+   */
+  public Rotation2d(double value) {
+    m_value = value;
+    m_cos = Math.cos(value);
+    m_sin = Math.sin(value);
+  }
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine)
+   * components.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  @SuppressWarnings("ParameterName")
+  public Rotation2d(double x, double y) {
+    double magnitude = Math.hypot(x, y);
+    if (magnitude > 1e-6) {
+      m_sin = y / magnitude;
+      m_cos = x / magnitude;
+    } else {
+      m_sin = 0.0;
+      m_cos = 1.0;
+    }
+    m_value = Math.atan2(m_sin, m_cos);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromDegrees(double degrees) {
+    return new Rotation2d(Math.toRadians(degrees));
+  }
+
+  /**
+   * Adds two rotations together, with the result being bounded between -pi and
+   * pi.
+   *
+   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
+   * Rotation2d{-pi/2}
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation2d plus(Rotation2d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
+   * Rotation2d{-pi/2}
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation2d minus(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of
+   * the current angular value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation2d unaryMinus() {
+    return new Rotation2d(-m_value);
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d times(double scalar) {
+    return new Rotation2d(m_value * scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <p>The matrix multiplication is as follows:
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = atan2(cos_new, sin_new)
+   *
+   * @param other The rotation to rotate by.
+   * @return The new rotated Rotation2d.
+   */
+  public Rotation2d rotateBy(Rotation2d other) {
+    return new Rotation2d(
+        m_cos * other.m_cos - m_sin * other.m_sin,
+        m_cos * other.m_sin + m_sin * other.m_cos
+    );
+  }
+
+  /*
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  public double getRadians() {
+    return m_value;
+  }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  public double getDegrees() {
+    return Math.toDegrees(m_value);
+  }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  public double getCos() {
+    return m_cos;
+  }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  public double getSin() {
+    return m_sin;
+  }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  public double getTan() {
+    return m_sin / m_cos;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+  }
+
+  /**
+   * Checks equality between this Rotation2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Rotation2d) {
+      return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_value);
+  }
+
+  static class RotationSerializer extends StdSerializer<Rotation2d> {
+    RotationSerializer() {
+      super(Rotation2d.class);
+    }
+
+    @Override
+    public void serialize(
+            Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
+            throws IOException, JsonProcessingException {
+
+      jgen.writeStartObject();
+      jgen.writeNumberField("radians", value.m_value);
+      jgen.writeEndObject();
+    }
+  }
+
+  static class RotationDeserializer extends StdDeserializer<Rotation2d> {
+    RotationDeserializer() {
+      super(Rotation2d.class);
+    }
+
+    @Override
+    public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
+            throws IOException, JsonProcessingException {
+      JsonNode node = jp.getCodec().readTree(jp);
+      double radians = node.get("radians").numberValue().doubleValue();
+
+      return new Rotation2d(radians);
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
new file mode 100644
index 0000000..507ebfe
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import java.util.Objects;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+public class Transform2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param last    The final pose for the transformation.
+   */
+  public Transform2d(Pose2d initial, Pose2d last) {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    m_translation = last.getTranslation().minus(initial.getTranslation())
+            .rotateBy(initial.getRotation().unaryMinus());
+
+    m_rotation = last.getRotation().minus(initial.getRotation());
+  }
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation    Rotational component of the transform.
+   */
+  public Transform2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  public Transform2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  public Transform2d times(double scalar) {
+    return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Transform2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Transform2d) {
+      return ((Transform2d) obj).m_translation.equals(m_translation)
+          && ((Transform2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
new file mode 100644
index 0000000..73b0257
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
@@ -0,0 +1,239 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * <p>This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+@JsonSerialize(using = Translation2d.TranslationSerializer.class)
+@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
+@SuppressWarnings({"ParameterName", "MemberName"})
+public class Translation2d {
+  private final double m_x;
+  private final double m_y;
+
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  public Translation2d() {
+    this(0.0, 0.0);
+  }
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  public Translation2d(double x, double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Calculates the distance between two translations in 2d space.
+   *
+   * <p>This function uses the pythagorean theorem to calculate the distance.
+   * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation2d other) {
+    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.hypot(m_x, m_y);
+  }
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * <p>This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+   * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+   * Translation2d of {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateBy(Rotation2d other) {
+    return new Translation2d(
+            m_x * other.getCos() - m_y * other.getSin(),
+            m_x * other.getSin() + m_y * other.getCos()
+    );
+  }
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to
+   * vector addition.
+   *
+   * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+   * Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation2d plus(Translation2d other) {
+    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+  }
+
+  /**
+   * Subtracts the other translation from the other translation and returns the
+   * difference.
+   *
+   * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}
+   *
+   * @param other The translation to subtract.
+   * @return The difference between the two translations.
+   */
+  public Translation2d minus(Translation2d other) {
+    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+  }
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * rotating by 180 degrees, flipping the point over both axes, or simply
+   * negating both components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation2d unaryMinus() {
+    return new Translation2d(-m_x, -m_y);
+  }
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled translation.
+   */
+  public Translation2d times(double scalar) {
+    return new Translation2d(m_x * scalar, m_y * scalar);
+  }
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation2d div(double scalar) {
+    return new Translation2d(m_x / scalar, m_y / scalar);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
+  /**
+   * Checks equality between this Translation2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Translation2d) {
+      return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
+          && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y);
+  }
+
+  static class TranslationSerializer extends StdSerializer<Translation2d> {
+    TranslationSerializer() {
+      super(Translation2d.class);
+    }
+
+    @Override
+    public void serialize(
+            Translation2d value, JsonGenerator jgen, SerializerProvider provider)
+            throws IOException, JsonProcessingException {
+
+      jgen.writeStartObject();
+      jgen.writeNumberField("x", value.m_x);
+      jgen.writeNumberField("y", value.m_y);
+      jgen.writeEndObject();
+    }
+  }
+
+  static class TranslationDeserializer extends StdDeserializer<Translation2d> {
+    TranslationDeserializer() {
+      super(Translation2d.class);
+    }
+
+    @Override
+    public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
+            throws IOException, JsonProcessingException {
+      JsonNode node = jp.getCodec().readTree(jp);
+      double xval = node.get("x").numberValue().doubleValue();
+      double yval = node.get("y").numberValue().doubleValue();
+
+      return new Translation2d(xval, yval);
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
new file mode 100644
index 0000000..f2def81
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import java.util.Objects;
+
+/**
+ * A change in distance along arc since the last pose update. We can use ideas
+ * from differential calculus to create new Pose2ds from a Twist2d and vise
+ * versa.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+@SuppressWarnings("MemberName")
+public class Twist2d {
+  /**
+   * Linear "dx" component.
+   */
+  public double dx;
+
+  /**
+   * Linear "dy" component.
+   */
+  public double dy;
+
+  /**
+   * Angular "dtheta" component (radians).
+   */
+  public double dtheta;
+
+  public Twist2d() {
+  }
+
+  /**
+   * Constructs a Twist2d with the given values.
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dtheta Change in angle relative to robot.
+   */
+  public Twist2d(double dx, double dy, double dtheta) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dtheta = dtheta;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+  }
+
+  /**
+   * Checks equality between this Twist2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Twist2d) {
+      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
+          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
+          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dtheta);
+  }
+}
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..a21e6d5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 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();
+}
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/kinematics/ChassisSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
new file mode 100644
index 0000000..aa98a47
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
+ * this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of
+ * reference.
+ *
+ * <p>A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+@SuppressWarnings("MemberName")
+public class ChassisSpeeds {
+  /**
+   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+   */
+  public double vxMetersPerSecond;
+
+  /**
+   * Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
+   */
+  public double vyMetersPerSecond;
+
+  /**
+   * Represents the angular velocity of the robot frame. (CCW is +)
+   */
+  public double omegaRadiansPerSecond;
+
+  /**
+   * Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
+   */
+  public ChassisSpeeds() {
+  }
+
+  /**
+   * Constructs a ChassisSpeeds object.
+   *
+   * @param vxMetersPerSecond     Forward velocity.
+   * @param vyMetersPerSecond     Sideways velocity.
+   * @param omegaRadiansPerSecond Angular velocity.
+   */
+  public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
+                       double omegaRadiansPerSecond) {
+    this.vxMetersPerSecond = vxMetersPerSecond;
+    this.vyMetersPerSecond = vyMetersPerSecond;
+    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
+  }
+
+  /**
+   * Converts a user provided field-relative set of speeds into a robot-relative
+   * ChassisSpeeds object.
+   *
+   * @param vxMetersPerSecond     The component of speed in the x direction relative to the field.
+   *                              Positive x is away from your alliance wall.
+   * @param vyMetersPerSecond     The component of speed in the y direction relative to the field.
+   *                              Positive y is to your left when standing behind the alliance wall.
+   * @param omegaRadiansPerSecond The angular rate of the robot.
+   * @param robotAngle            The angle of the robot as measured by a gyroscope. The robot's
+   *                              angle is considered to be zero when it is facing directly away
+   *                              from your alliance station wall. Remember that this should
+   *                              be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
+   */
+  public static ChassisSpeeds fromFieldRelativeSpeeds(
+      double vxMetersPerSecond, double vyMetersPerSecond,
+      double omegaRadiansPerSecond, Rotation2d robotAngle) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
+        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
+        omegaRadiansPerSecond
+    );
+  }
+
+  @Override
+  public String toString() {
+    return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
new file mode 100644
index 0000000..bb632af
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * <p>Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveKinematics {
+  public final double trackWidthMeters;
+
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is
+   *                         the distance between the left wheels and right wheels.
+   *                         However, the empirical value may be larger than the physical
+   *                         measured value due to scrubbing effects.
+   */
+  public DifferentialDriveKinematics(double trackWidthMeters) {
+    this.trackWidthMeters = trackWidthMeters;
+    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_DifferentialDrive);
+  }
+
+  /**
+   * Returns a chassis speed from left and right component velocities using
+   * forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
+    return new ChassisSpeeds(
+        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
+        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
+            / trackWidthMeters
+    );
+  }
+
+  /**
+   * Returns left and right component velocities from a chassis speed using
+   * inverse kinematics.
+   *
+   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+   *                      represent the chassis' speed.
+   * @return The left and right velocities.
+   */
+  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new DifferentialDriveWheelSpeeds(
+        chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
+          * chassisSpeeds.omegaRadiansPerSecond,
+        chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2
+          * chassisSpeeds.omegaRadiansPerSecond
+    );
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
new file mode 100644
index 0000000..35a4342
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+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.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * <p>It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
+ */
+public class DifferentialDriveOdometry {
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private double m_prevLeftDistance;
+  private double m_prevRightDistance;
+
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle         The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle,
+                                   Pose2d initialPoseMeters) {
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
+  }
+
+  /**
+   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
+    this(gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+
+    m_prevLeftDistance = 0.0;
+    m_prevRightDistance = 0.0;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+
+  /**
+   * Updates the robot position on the field using distance measurements from encoders. This
+   * method is more numerically accurate than using velocities to integrate the pose and
+   * is also advantageous for teams that are using lower CPR encoders.
+   *
+   * @param gyroAngle           The angle reported by the gyroscope.
+   * @param leftDistanceMeters  The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
+                       double rightDistanceMeters) {
+    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
+
+    m_prevLeftDistance = leftDistanceMeters;
+    m_prevRightDistance = rightDistanceMeters;
+
+    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var newPose = m_poseMeters.exp(
+        new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
new file mode 100644
index 0000000..ac4a9fa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+/**
+ * Represents the wheel speeds for a differential drive drivetrain.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveWheelSpeeds {
+  /**
+   * Speed of the left side of the robot.
+   */
+  public double leftMetersPerSecond;
+
+  /**
+   * Speed of the right side of the robot.
+   */
+  public double rightMetersPerSecond;
+
+  /**
+   * Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
+   */
+  public DifferentialDriveWheelSpeeds() {
+  }
+
+  /**
+   * Constructs a DifferentialDriveWheelSpeeds.
+   *
+   * @param leftMetersPerSecond  The left speed.
+   * @param rightMetersPerSecond The right speed.
+   */
+  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
+    this.leftMetersPerSecond = leftMetersPerSecond;
+    this.rightMetersPerSecond = rightMetersPerSecond;
+  }
+
+  /**
+   * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+   * after inverse kinematics, the requested speed from a/several modules may be
+   * above the max attainable speed for the driving motor on that module. To fix
+   * this issue, one can "normalize" all the wheel speeds to make sure that all
+   * requested module speeds are below the absolute threshold, while maintaining
+   * the ratio of speeds between modules.
+   *
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+        leftMetersPerSecond, rightMetersPerSecond);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
new file mode 100644
index 0000000..a5e5b5f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
@@ -0,0 +1,173 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.ejml.simple.SimpleMatrix;
+
+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.geometry.Translation2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds.
+ *
+ * <p>The inverse kinematics (converting from a desired chassis velocity to
+ * individual wheel speeds) uses the relative locations of the wheels with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion manuevers.
+ *
+ * <p>Forward kinematics (converting an array of wheel speeds into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
+ * multiply by [wheelSpeeds] to get our chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+public class MecanumDriveKinematics {
+  private SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final Translation2d m_frontLeftWheelMeters;
+  private final Translation2d m_frontRightWheelMeters;
+  private final Translation2d m_rearLeftWheelMeters;
+  private final Translation2d m_rearRightWheelMeters;
+
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * Constructs a mecanum drive kinematics object.
+   *
+   * @param frontLeftWheelMeters  The location of the front-left wheel relative to the
+   *                              physical center of the robot.
+   * @param frontRightWheelMeters The location of the front-right wheel relative to
+   *                              the physical center of the robot.
+   * @param rearLeftWheelMeters   The location of the rear-left wheel relative to the
+   *                              physical center of the robot.
+   * @param rearRightWheelMeters  The location of the rear-right wheel relative to the
+   *                              physical center of the robot.
+   */
+  public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
+                                Translation2d frontRightWheelMeters,
+                                Translation2d rearLeftWheelMeters,
+                                Translation2d rearRightWheelMeters) {
+    m_frontLeftWheelMeters = frontLeftWheelMeters;
+    m_frontRightWheelMeters = frontRightWheelMeters;
+    m_rearLeftWheelMeters = rearLeftWheelMeters;
+    m_rearRightWheelMeters = rearRightWheelMeters;
+
+    m_inverseKinematics = new SimpleMatrix(4, 3);
+
+    setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
+        rearLeftWheelMeters, rearRightWheelMeters);
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
+  }
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+   * method is often used to convert joystick values into wheel speeds.
+   *
+   * <p>This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * manuevers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds    The desired chassis speed.
+   * @param centerOfRotationMeters The center of rotation. For example, if you set the
+   *                         center of rotation at one corner of the robot and provide
+   *                         a chassis speed that only has a dtheta component, the robot
+   *                         will rotate around that corner.
+   * @return  The wheel speeds. Use caution because they are not normalized. Sometimes, a user
+   *          input may cause one of the wheel speeds to go above the attainable max velocity. Use
+   *          the {@link MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
+                                               Translation2d centerOfRotationMeters) {
+    // We have a new center of rotation. We need to compute the matrix again.
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
+      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
+      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
+      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
+
+      setInverseKinematics(fl, fr, rl, rr);
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(0, 0,
+        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    return new MecanumDriveWheelSpeeds(
+        wheelsMatrix.get(0, 0),
+        wheelsMatrix.get(1, 0),
+        wheelsMatrix.get(2, 0),
+        wheelsMatrix.get(3, 0)
+    );
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
+   * information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return toWheelSpeeds(chassisSpeeds, new Translation2d());
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
+   * This method is often used for odometry -- determining the robot's position on the field using
+   * data from the real-world speed of each wheel on the robot.
+   *
+   * @param wheelSpeeds The current mecanum drive wheel speeds.
+   * @return The resulting chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+    var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
+    wheelSpeedsMatrix.setColumn(0, 0,
+        wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
+        wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
+    );
+    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
+
+    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+  }
+
+  /**
+   * Construct inverse kinematics matrix from wheel locations.
+   *
+   * @param fl The location of the front-left wheel relative to the physical center of the robot.
+   * @param fr The location of the front-right wheel relative to the physical center of the robot.
+   * @param rl The location of the rear-left wheel relative to the physical center of the robot.
+   * @param rr The location of the rear-right wheel relative to the physical center of the robot.
+   */
+  private void setInverseKinematics(Translation2d fl, Translation2d fr,
+                                    Translation2d rl, Translation2d rr) {
+    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
+    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
+    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
+    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
+    m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
new file mode 100644
index 0000000..f467d20
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+/**
+ * Represents the motor voltages for a mecanum drive drivetrain.
+ */
+@SuppressWarnings("MemberName")
+public class MecanumDriveMotorVoltages {
+  /**
+   * Voltage of the front left motor.
+   */
+  public double frontLeftVoltage;
+
+  /**
+   * Voltage of the front right motor.
+   */
+  public double frontRightVoltage;
+
+  /**
+   * Voltage of the rear left motor.
+   */
+  public double rearLeftVoltage;
+
+  /**
+   * Voltage of the rear right motor.
+   */
+  public double rearRightVoltage;
+
+  /**
+   * Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
+   */
+  public MecanumDriveMotorVoltages() {
+  }
+
+  /**
+   * Constructs a MecanumDriveMotorVoltages.
+   *
+   * @param frontLeftVoltage  Voltage of the front left motor.
+   * @param frontRightVoltage Voltage of the front right motor.
+   * @param rearLeftVoltage   Voltage of the rear left motor.
+   * @param rearRightVoltage  Voltage of the rear right motor.
+   */
+  public MecanumDriveMotorVoltages(double frontLeftVoltage,
+                                 double frontRightVoltage,
+                                 double rearLeftVoltage,
+                                 double rearRightVoltage) {
+    this.frontLeftVoltage = frontLeftVoltage;
+    this.frontRightVoltage = frontRightVoltage;
+    this.rearLeftVoltage = rearLeftVoltage;
+    this.rearRightVoltage = rearRightVoltage;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+            + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
+        rearLeftVoltage, rearRightVoltage);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
new file mode 100644
index 0000000..b3dd402
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+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.Timer;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * mecanum wheel encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+public class MecanumDriveOdometry {
+  private final MecanumDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a MecanumDriveOdometry object.
+   *
+   * @param kinematics        The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle         The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
+                              Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
+  }
+
+  /**
+   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTimeSeconds The current time in seconds.
+   * @param gyroAngle          The angle reported by the gyroscope.
+   * @param wheelSpeeds        The current wheel speeds.
+   * @return The new pose of the robot.
+   */
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+                               MecanumDriveWheelSpeeds wheelSpeeds) {
+    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
+    var newPose = m_poseMeters.exp(
+        new Twist2d(chassisState.vxMetersPerSecond * period,
+            chassisState.vyMetersPerSecond * period,
+            angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates the
+   * current time to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle   The angle reported by the gyroscope.
+   * @param wheelSpeeds The current wheel speeds.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle,
+                       MecanumDriveWheelSpeeds wheelSpeeds) {
+    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
+        wheelSpeeds);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
new file mode 100644
index 0000000..0f0dd71
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import java.util.stream.DoubleStream;
+
+@SuppressWarnings("MemberName")
+public class MecanumDriveWheelSpeeds {
+  /**
+   * Speed of the front left wheel.
+   */
+  public double frontLeftMetersPerSecond;
+
+  /**
+   * Speed of the front right wheel.
+   */
+  public double frontRightMetersPerSecond;
+
+  /**
+   * Speed of the rear left wheel.
+   */
+  public double rearLeftMetersPerSecond;
+
+  /**
+   * Speed of the rear right wheel.
+   */
+  public double rearRightMetersPerSecond;
+
+  /**
+   * Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
+   */
+  public MecanumDriveWheelSpeeds() {
+  }
+
+  /**
+   * Constructs a MecanumDriveWheelSpeeds.
+   *
+   * @param frontLeftMetersPerSecond  Speed of the front left wheel.
+   * @param frontRightMetersPerSecond Speed of the front right wheel.
+   * @param rearLeftMetersPerSecond   Speed of the rear left wheel.
+   * @param rearRightMetersPerSecond  Speed of the rear right wheel.
+   */
+  public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
+                                 double frontRightMetersPerSecond,
+                                 double rearLeftMetersPerSecond,
+                                 double rearRightMetersPerSecond) {
+    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
+    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
+    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
+    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
+  }
+
+  /**
+   * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+   * after inverse kinematics, the requested speed from a/several modules may be
+   * above the max attainable speed for the driving motor on that module. To fix
+   * this issue, one can "normalize" all the wheel speeds to make sure that all
+   * requested module speeds are below the absolute threshold, while maintaining
+   * the ratio of speeds between modules.
+   *
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
+        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
+        .max().getAsDouble();
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
+        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
new file mode 100644
index 0000000..37f97b8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
@@ -0,0 +1,200 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import java.util.Arrays;
+import java.util.Collections;
+
+import org.ejml.simple.SimpleMatrix;
+
+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.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual module states (speed and angle).
+ *
+ * <p>The inverse kinematics (converting from a desired chassis velocity to
+ * individual module states) uses the relative locations of the modules with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion manuevers.
+ *
+ * <p>Forward kinematics (converting an array of module states into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
+ * multiply by [moduleStates] to get our chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+public class SwerveDriveKinematics {
+  private final SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final int m_numModules;
+  private final Translation2d[] m_modules;
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * Constructs a swerve drive kinematics object. This takes in a variable
+   * number of wheel locations as Translation2ds. The order in which you pass in
+   * the wheel locations is the same order that you will recieve the module
+   * states when performing inverse kinematics. It is also expected that you
+   * pass in the module states in the same order when calling the forward
+   * kinematics methods.
+   *
+   * @param wheelsMeters The locations of the wheels relative to the physical center
+   *                     of the robot.
+   */
+  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
+    if (wheelsMeters.length < 2) {
+      throw new IllegalArgumentException("A swerve drive requires at least two modules");
+    }
+    m_numModules = wheelsMeters.length;
+    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+
+    for (int i = 0; i < m_numModules; i++) {
+      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
+      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
+    }
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
+  }
+
+  /**
+   * Performs inverse kinematics to return the module states from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * module speeds and angles.
+   *
+   * <p>This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * manuevers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds    The desired chassis speed.
+   * @param centerOfRotationMeters The center of rotation. For example, if you set the
+   *                         center of rotation at one corner of the robot and provide
+   *                         a chassis speed that only has a dtheta component, the robot
+   *                         will rotate around that corner.
+   * @return  An array containing the module states. Use caution because these
+   *          module states are not normalized. Sometimes, a user input may cause one of
+   *          the module speeds to go above the attainable max velocity. Use the
+   *          {@link #normalizeWheelSpeeds(SwerveModuleState[], double) normalizeWheelSpeeds}
+   *          function to rectify this issue.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
+                                                  Translation2d centerOfRotationMeters) {
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      for (int i = 0; i < m_numModules; i++) {
+        m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
+            -m_modules[i].getY() + centerOfRotationMeters.getY());
+        m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
+            +m_modules[i].getX() - centerOfRotationMeters.getX());
+      }
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(0, 0,
+        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
+
+    for (int i = 0; i < m_numModules; i++) {
+      double x = moduleStatesMatrix.get(i * 2, 0);
+      double y = moduleStatesMatrix.get(i * 2 + 1, 0);
+
+      double speed = Math.hypot(x, y);
+      Rotation2d angle = new Rotation2d(x, y);
+
+      moduleStates[i] = new SwerveModuleState(speed, angle);
+    }
+
+    return moduleStates;
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
+   * toSwerveModuleStates for more information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return An array containing the module states.
+   */
+  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
+    return toSwerveModuleStates(chassisSpeeds, new Translation2d());
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given module states. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed and
+   * angle of each module on the robot.
+   *
+   * @param wheelStates The state of the modules (as a SwerveModuleState type)
+   *                    as measured from respective encoders and gyros. The order of the swerve
+   *                    module states should be same as passed into the constructor of this class.
+   * @return The resulting chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
+    if (wheelStates.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor"
+      );
+    }
+    var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelStates[i];
+      moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
+      moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
+    }
+
+    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
+    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+
+  }
+
+  /**
+   * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+   * after inverse kinematics, the requested speed from a/several modules may be
+   * above the max attainable speed for the driving motor on that module. To fix
+   * this issue, one can "normalize" all the wheel speeds to make sure that all
+   * requested module speeds are below the absolute threshold, while maintaining
+   * the ratio of speeds between modules.
+   *
+   * @param moduleStates       Reference to array of module states. The array will be
+   *                           mutated with the normalized speeds!
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
+   */
+  public static void normalizeWheelSpeeds(SwerveModuleState[] moduleStates,
+                                          double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      for (SwerveModuleState moduleState : moduleStates) {
+        moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
+            * attainableMaxSpeedMetersPerSecond;
+      }
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
new file mode 100644
index 0000000..811c3de
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+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.Timer;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+public class SwerveDriveOdometry {
+  private final SwerveDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a SwerveDriveOdometry object.
+   *
+   * @param kinematics  The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle   The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
+                             Pose2d initialPose) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPose;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPose.getRotation();
+    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
+  }
+
+  /**
+   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose      The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+    m_poseMeters = pose;
+    m_previousAngle = pose.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTimeSeconds The current time in seconds.
+   * @param gyroAngle          The angle reported by the gyroscope.
+   * @param moduleStates       The current state of all swerve modules. Please provide
+   *                           the states in the same order in which you instantiated your
+   *                           SwerveDriveKinematics.
+   * @return The new pose of the robot.
+   */
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+                               SwerveModuleState... moduleStates) {
+    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
+    var newPose = m_poseMeters.exp(
+        new Twist2d(chassisState.vxMetersPerSecond * period,
+            chassisState.vyMetersPerSecond * period,
+            angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates the
+   * current time to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the angular
+   * rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle    The angle reported by the gyroscope.
+   * @param moduleStates The current state of all swerve modules. Please provide
+   *                     the states in the same order in which you instantiated your
+   *                     SwerveDriveKinematics.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
new file mode 100644
index 0000000..e3119e5
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents the state of one swerve module.
+ */
+@SuppressWarnings("MemberName")
+public class SwerveModuleState implements Comparable<SwerveModuleState> {
+
+  /**
+   * Speed of the wheel of the module.
+   */
+  public double speedMetersPerSecond;
+
+  /**
+   * Angle of the module.
+   */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /**
+   * Constructs a SwerveModuleState with zeros for speed and angle.
+   */
+  public SwerveModuleState() {
+  }
+
+  /**
+   * Constructs a SwerveModuleState.
+   *
+   * @param speedMetersPerSecond The speed of the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
+    this.speedMetersPerSecond = speedMetersPerSecond;
+    this.angle = angle;
+  }
+
+  /**
+   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
+   * is higher than the other.
+   *
+   * @param o The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  @SuppressWarnings("ParameterName")
+  public int compareTo(SwerveModuleState o) {
+    return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
+        angle);
+  }
+}
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..5a4fb36
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.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.smartdashboard.SendableRegistry;
+
+
+/**
+ * The LiveWindow class is the public interface for putting sensors and actuators on the
+ * LiveWindow.
+ */
+public class LiveWindow {
+  private static class Component {
+    boolean m_firstTime = true;
+    boolean m_telemetryEnabled = true;
+  }
+
+  private static final int dataHandle = SendableRegistry.getDataHandle();
+  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 static Component getOrAdd(Sendable sendable) {
+    Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
+    if (data == null) {
+      data = new Component();
+      SendableRegistry.setData(sendable, dataHandle, data);
+    }
+    return data;
+  }
+
+  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
+      if (enabled) {
+        System.out.println("Starting live window mode.");
+      } else {
+        System.out.println("stopping live window mode.");
+        SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
+          cbdata.builder.stopLiveWindowMode();
+        });
+      }
+      enabledEntry.setBoolean(enabled);
+    }
+  }
+
+  /**
+   * 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;
+    getOrAdd(sendable).m_telemetryEnabled = true;
+  }
+
+  /**
+   * Disable telemetry for a single component.
+   *
+   * @param sendable component
+   */
+  public static synchronized void disableTelemetry(Sendable sendable) {
+    getOrAdd(sendable).m_telemetryEnabled = false;
+  }
+
+  /**
+   * Disable ALL telemetry.
+   */
+  public static synchronized void disableAllTelemetry() {
+    telemetryEnabled = false;
+    SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
+      if (cbdata.data == null) {
+        cbdata.data = new Component();
+      }
+      ((Component) cbdata.data).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", "PMD.NPathComplexity"})
+  public static synchronized void updateValues() {
+    // Only do this if either LiveWindow mode or telemetry is enabled.
+    if (!liveWindowEnabled && !telemetryEnabled) {
+      return;
+    }
+
+    SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
+      if (cbdata.sendable == null || cbdata.parent != null) {
+        return;
+      }
+
+      if (cbdata.data == null) {
+        cbdata.data = new Component();
+      }
+
+      Component component = (Component) cbdata.data;
+
+      if (!liveWindowEnabled && !component.m_telemetryEnabled) {
+        return;
+      }
+
+      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.
+        if (cbdata.name.isEmpty()) {
+          return;
+        }
+        NetworkTable ssTable = liveWindowTable.getSubTable(cbdata.subsystem);
+        NetworkTable table;
+        // Treat name==subsystem as top level of subsystem
+        if (cbdata.name.equals(cbdata.subsystem)) {
+          table = ssTable;
+        } else {
+          table = ssTable.getSubTable(cbdata.name);
+        }
+        table.getEntry(".name").setString(cbdata.name);
+        cbdata.builder.setTable(table);
+        cbdata.sendable.initSendable(cbdata.builder);
+        ssTable.getEntry(".type").setString("LW Subsystem");
+
+        component.m_firstTime = false;
+      }
+
+      if (startLiveWindow) {
+        cbdata.builder.startLiveWindowMode();
+      }
+      cbdata.builder.updateTable();
+    });
+
+    startLiveWindow = false;
+  }
+}
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..a18e4c8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 java.util.function.BiConsumer;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A helper class for Shuffleboard containers to handle common child operations.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+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 {
+    String name = SendableRegistry.getName(sendable);
+    if (name.isEmpty()) {
+      throw new IllegalArgumentException("Sendable must have a name");
+    }
+    return add(name, 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;
+  }
+
+  SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier, NetworkTableEntry::setString);
+  }
+
+  SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier::getAsDouble, NetworkTableEntry::setDouble);
+  }
+
+  SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier::getAsBoolean, NetworkTableEntry::setBoolean);
+  }
+
+  SuppliedValueWidget<String[]> addStringArray(String title, Supplier<String[]> valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier, NetworkTableEntry::setStringArray);
+  }
+
+  SuppliedValueWidget<double[]> addDoubleArray(String title, Supplier<double[]> valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier, NetworkTableEntry::setDoubleArray);
+  }
+
+  SuppliedValueWidget<boolean[]> addBooleanArray(String title, Supplier<boolean[]> valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier, NetworkTableEntry::setBooleanArray);
+  }
+
+  SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier) {
+    precheck(title, valueSupplier);
+    return addSupplied(title, valueSupplier, NetworkTableEntry::setRaw);
+  }
+
+  private void precheck(String title, Object valueSupplier) {
+    Objects.requireNonNull(title, "Title cannot be null");
+    Objects.requireNonNull(valueSupplier, "Value supplier cannot be null");
+    checkTitle(title);
+  }
+
+  private <T> SuppliedValueWidget<T> addSupplied(String title,
+                                                 Supplier<T> supplier,
+                                                 BiConsumer<NetworkTableEntry, T> setter) {
+    SuppliedValueWidget<T> widget = new SuppliedValueWidget<>(m_container, title, supplier, setter);
+    m_components.add(widget);
+    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..bb7738f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A wrapper to make video sources sendable and usable from Shuffleboard.
+ */
+public final class SendableCameraWrapper implements Sendable, AutoCloseable {
+  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) {
+    String name = source.getName();
+    SendableRegistry.add(this, name);
+    m_uri = kProtocol + name;
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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..c88256a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
@@ -0,0 +1,198 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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:</p>
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ *   .add("My Boolean", false)
+ *   .withWidget("Toggle Button")
+ *   .getEntry();
+ * }</pre>
+ *
+ * <p>Changing the colors of the boolean box:</p>
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ *   .add("My Boolean", false)
+ *   .withWidget("Boolean Box")
+ *   .withProperties(Map.of("colorWhenTrue", "green", "colorWhenFalse", "maroon"))
+ *   .getEntry();
+ * }</pre>
+ *
+ * <p>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.</p>
+ * <pre>{@code
+ * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
+ *   .getLayout("List", "Example List")
+ *   .add("My Boolean", false)
+ *   .withWidget("Toggle Button")
+ *   .getEntry();
+ * }</pre>
+ *
+ * <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());
+
+  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..5d90396
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 edu.wpi.first.networktables.NetworkTable;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 = requireNonNullParam(parent, "parent", "ShuffleboardComponent");
+    m_title = requireNonNullParam(title, "title", "ShuffleboardComponent");
+    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..331b7f1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
@@ -0,0 +1,248 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * Common interface for objects that can contain shuffleboard components.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+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. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier)
+      throws IllegalArgumentException;
+
+  /**
+   * Adds a widget to this container. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier)
+      throws IllegalArgumentException;
+
+  /**
+   * Adds a widget to this container. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier)
+      throws IllegalArgumentException;
+
+  /**
+   * Adds a widget to this container. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<String[]> addStringArray(String title, Supplier<String[]> valueSupplier)
+      throws IllegalArgumentException;
+
+  /**
+   * Adds a widget to this container. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<double[]> addDoubleArray(String title, Supplier<double[]> valueSupplier)
+      throws IllegalArgumentException;
+
+  /**
+   * Adds a widget to this container. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<boolean[]> addBooleanArray(String title, Supplier<boolean[]> valueSupplier)
+      throws IllegalArgumentException;
+
+  /**
+   * Adds a widget to this container. The widget will display the data provided by the value
+   * supplier. Changes made on the dashboard will not propagate to the widget object, and will be
+   * overridden by values from the value supplier.
+   *
+   * @param title the title of the widget
+   * @param valueSupplier the supplier for values
+   * @return a widget to display data
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *                                  title
+   */
+  SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier)
+      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..8af1d78
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.function.Consumer;
+
+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 static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+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;
+
+  /**
+   * Creates a new Shuffleboard instance.
+   *
+   * @param ntInstance the NetworkTables instance to use
+   */
+  ShuffleboardInstance(NetworkTableInstance ntInstance) {
+    requireNonNullParam(ntInstance, "ntInstance", "ShuffleboardInstance");
+    m_rootTable = ntInstance.getTable(Shuffleboard.kBaseTableName);
+    m_rootMetaTable = m_rootTable.getSubTable(".metadata");
+    m_selectedTabEntry = m_rootMetaTable.getEntry("Selected");
+    HAL.report(tResourceType.kResourceType_Shuffleboard, 0);
+  }
+
+  @Override
+  public ShuffleboardTab getTab(String title) {
+    requireNonNullParam(title, "title", "getTab");
+    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..8f3014a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A layout in a Shuffleboard tab. Layouts can contain widgets and other layouts.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+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, requireNonNullParam(type, "type", "ShuffleboardLayout"), 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 SuppliedValueWidget<String> addString(String title,
+                                               Supplier<String> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addString(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<Double> addNumber(String title,
+                                               DoubleSupplier valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addNumber(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<Boolean> addBoolean(String title,
+                                                 BooleanSupplier valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addBoolean(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<String[]> addStringArray(String title,
+                                                      Supplier<String[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addStringArray(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<double[]> addDoubleArray(String title,
+                                                      Supplier<double[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addDoubleArray(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<boolean[]> addBooleanArray(String title,
+                                                        Supplier<boolean[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addBooleanArray(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<byte[]> addRaw(String title,
+                                            Supplier<byte[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addRaw(title, valueSupplier);
+  }
+
+  @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..b46c6d2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+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).
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+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 SuppliedValueWidget<String> addString(String title,
+                                               Supplier<String> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addString(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<Double> addNumber(String title,
+                                               DoubleSupplier valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addNumber(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<Boolean> addBoolean(String title,
+                                                 BooleanSupplier valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addBoolean(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<String[]> addStringArray(String title,
+                                                      Supplier<String[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addStringArray(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<double[]> addDoubleArray(String title,
+                                                      Supplier<double[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addDoubleArray(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<boolean[]> addBooleanArray(String title,
+                                                        Supplier<boolean[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addBooleanArray(title, valueSupplier);
+  }
+
+  @Override
+  public SuppliedValueWidget<byte[]> addRaw(String title,
+                                            Supplier<byte[]> valueSupplier)
+      throws IllegalArgumentException {
+    return m_helper.addRaw(title, valueSupplier);
+  }
+
+  @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/SuppliedValueWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
new file mode 100644
index 0000000..c7c9e8b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.function.BiConsumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+/**
+ * A Shuffleboard widget whose value is provided by user code.
+ *
+ * @param <T> the type of values in the widget
+ */
+public final class SuppliedValueWidget<T> extends ShuffleboardWidget<SuppliedValueWidget<T>> {
+  private final Supplier<T> m_supplier;
+  private final BiConsumer<NetworkTableEntry, T> m_setter;
+
+  /**
+   * Package-private constructor for use by the Shuffleboard API.
+   *
+   * @param parent   the parent container for the widget
+   * @param title    the title of the widget
+   * @param supplier the supplier for values to place in the NetworkTable entry
+   * @param setter   the function for placing values in the NetworkTable entry
+   */
+  SuppliedValueWidget(ShuffleboardContainer parent,
+                      String title,
+                      Supplier<T> supplier,
+                      BiConsumer<NetworkTableEntry, T> setter) {
+    super(parent, title);
+    this.m_supplier = supplier;
+    this.m_setter = setter;
+  }
+
+  @Override
+  public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
+    buildMetadata(metaTable);
+    metaTable.getEntry("Controllable").setBoolean(false);
+
+    NetworkTableEntry entry = parentTable.getEntry(getTitle());
+    m_setter.accept(entry, m_supplier.get());
+  }
+}
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/ListenerExecutor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
new file mode 100644
index 0000000..274c7a8
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.Collection;
+import java.util.concurrent.Executor;
+
+/**
+ * An executor for running listener tasks posted by {@link edu.wpi.first.wpilibj.Sendable} listeners
+ * synchronously from the main application thread.
+ */
+class ListenerExecutor implements Executor {
+  private final Collection<Runnable> m_tasks = new ArrayList<>();
+  private final Object m_lock = new Object();
+
+  /**
+   * Posts a task to the executor to be run synchronously from the main thread.
+   *
+   * @param task The task to run synchronously from the main thread.
+   */
+  @Override
+  public void execute(Runnable task) {
+    synchronized (m_lock) {
+      m_tasks.add(task);
+    }
+  }
+
+  /**
+   * Runs all posted tasks.  Called periodically from main thread.
+   */
+  public void runListenerTasks() {
+    // Locally copy tasks from internal list; minimizes blocking time
+    Collection<Runnable> tasks = new ArrayList<>();
+    synchronized (m_lock) {
+      tasks.addAll(m_tasks);
+      m_tasks.clear();
+    }
+
+    // Run all tasks
+    for (Runnable task : tasks) {
+      task.run();
+    }
+  }
+}
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..e0bb4f9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
@@ -0,0 +1,410 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 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 has an associated table.
+   * @return True if it has a table, false if not.
+   */
+  public boolean hasTable() {
+    return m_table != null;
+  }
+
+  /**
+   * 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();
+    }
+  }
+
+  /**
+   * Clear properties.
+   */
+  public void clearProperties() {
+    stopListeners();
+    m_properties.clear();
+  }
+
+  /**
+   * 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()) {
+          SmartDashboard.postListenerTask(() -> 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()) {
+          SmartDashboard.postListenerTask(() -> 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()) {
+          SmartDashboard.postListenerTask(() -> 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()) {
+          SmartDashboard.postListenerTask(() -> 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()) {
+          SmartDashboard.postListenerTask(() -> 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()) {
+          SmartDashboard.postListenerTask(() -> 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()) {
+          SmartDashboard.postListenerTask(() -> 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 -> {
+        SmartDashboard.postListenerTask(() -> 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..cdf1ffb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.Sendable;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * 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 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> implements Sendable, AutoCloseable {
+  /**
+   * 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.
+   */
+  private final Map<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() {
+    m_instance = s_instances.getAndIncrement();
+    SendableRegistry.add(this, "SendableChooser", m_instance);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * 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) {
+    requireNonNullParam(name, "name", "setDefaultOption");
+
+    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, () -> m_defaultChoice, null);
+    builder.addStringArrayProperty(OPTIONS, () -> 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/SendableRegistry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
new file mode 100644
index 0000000..a8cd557
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
@@ -0,0 +1,521 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.lang.ref.WeakReference;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.Map;
+import java.util.WeakHashMap;
+import java.util.function.Consumer;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Sendable;
+
+
+/**
+ * The SendableRegistry class is the public interface for registering sensors
+ * and actuators for use on dashboards and LiveWindow.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class SendableRegistry {
+  private static class Component {
+    Component() {}
+
+    Component(Sendable sendable) {
+      m_sendable = new WeakReference<>(sendable);
+    }
+
+    WeakReference<Sendable> m_sendable;
+    SendableBuilderImpl m_builder = new SendableBuilderImpl();
+    String m_name;
+    String m_subsystem = "Ungrouped";
+    WeakReference<Sendable> m_parent;
+    boolean m_liveWindow;
+    Object[] m_data;
+
+    void setName(String moduleType, int channel) {
+      m_name = moduleType + "[" + channel + "]";
+    }
+
+    void setName(String moduleType, int moduleNumber, int channel) {
+      m_name = moduleType + "[" + moduleNumber + "," + channel + "]";
+    }
+  }
+
+  private static final Map<Object, Component> components = new WeakHashMap<>();
+  private static int nextDataHandle;
+
+  private static Component getOrAdd(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      comp = new Component(sendable);
+      components.put(sendable, comp);
+    } else {
+      if (comp.m_sendable == null) {
+        comp.m_sendable = new WeakReference<>(sendable);
+      }
+    }
+    return comp;
+  }
+
+  private SendableRegistry() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param name component name
+   */
+  public static synchronized void add(Sendable sendable, String name) {
+    Component comp = getOrAdd(sendable);
+    comp.m_name = name;
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable     object to add
+   * @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 static synchronized void add(Sendable sendable, String moduleType, int channel) {
+    Component comp = getOrAdd(sendable);
+    comp.setName(moduleType, channel);
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable     object to add
+   * @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
+   */
+  public static synchronized void add(Sendable sendable, String moduleType, int moduleNumber,
+      int channel) {
+    Component comp = getOrAdd(sendable);
+    comp.setName(moduleType, moduleNumber, channel);
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param subsystem subsystem name
+   * @param name component name
+   */
+  public static synchronized void add(Sendable sendable, String subsystem, String name) {
+    Component comp = getOrAdd(sendable);
+    comp.m_name = name;
+    comp.m_subsystem = subsystem;
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param name component name
+   */
+  public static synchronized void addLW(Sendable sendable, String name) {
+    Component comp = getOrAdd(sendable);
+    comp.m_liveWindow = true;
+    comp.m_name = name;
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable     object to add
+   * @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 static synchronized void addLW(Sendable sendable, String moduleType, int channel) {
+    Component comp = getOrAdd(sendable);
+    comp.m_liveWindow = true;
+    comp.setName(moduleType, channel);
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable     object to add
+   * @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
+   */
+  public static synchronized void addLW(Sendable sendable, String moduleType, int moduleNumber,
+      int channel) {
+    Component comp = getOrAdd(sendable);
+    comp.m_liveWindow = true;
+    comp.setName(moduleType, moduleNumber, channel);
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param subsystem subsystem name
+   * @param name component name
+   */
+  public static synchronized void addLW(Sendable sendable, String subsystem, String name) {
+    Component comp = getOrAdd(sendable);
+    comp.m_liveWindow = true;
+    comp.m_name = name;
+    comp.m_subsystem = subsystem;
+  }
+
+  /**
+   * Adds a child object to an object.  Adds the child object to the registry
+   * if it's not already present.
+   *
+   * @param parent parent object
+   * @param child child object
+   */
+  public static synchronized void addChild(Sendable parent, Object child) {
+    Component comp = components.get(child);
+    if (comp == null) {
+      comp = new Component();
+      components.put(child, comp);
+    }
+    comp.m_parent = new WeakReference<>(parent);
+  }
+
+  /**
+   * Removes an object from the registry.
+   *
+   * @param sendable object to remove
+   * @return true if the object was removed; false if it was not present
+   */
+  public static synchronized boolean remove(Sendable sendable) {
+    return components.remove(sendable) != null;
+  }
+
+  /**
+   * Determines if an object is in the registry.
+   *
+   * @param sendable object to check
+   * @return True if in registry, false if not.
+   */
+  public static synchronized boolean contains(Sendable sendable) {
+    return components.containsKey(sendable);
+  }
+
+  /**
+   * Gets the name of an object.
+   *
+   * @param sendable object
+   * @return Name (empty if object is not in registry)
+   */
+  public static synchronized String getName(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      return "";
+    }
+    return comp.m_name;
+  }
+
+  /**
+   * Sets the name of an object.
+   *
+   * @param sendable object
+   * @param name name
+   */
+  public static synchronized void setName(Sendable sendable, String name) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_name = name;
+    }
+  }
+
+  /**
+   * Sets the name of an object with a channel number.
+   *
+   * @param sendable   object
+   * @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 static synchronized void setName(Sendable sendable, String moduleType, int channel) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.setName(moduleType, channel);
+    }
+  }
+
+  /**
+   * Sets the name of an object with a module and channel number.
+   *
+   * @param sendable     object
+   * @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
+   */
+  public static synchronized void setName(Sendable sendable, String moduleType, int moduleNumber,
+      int channel) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.setName(moduleType, moduleNumber, channel);
+    }
+  }
+
+  /**
+   * Sets both the subsystem name and device name of an object.
+   *
+   * @param sendable object
+   * @param subsystem subsystem name
+   * @param name device name
+   */
+  public static synchronized void setName(Sendable sendable, String subsystem, String name) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_name = name;
+      comp.m_subsystem = subsystem;
+    }
+  }
+
+  /**
+   * Gets the subsystem name of an object.
+   *
+   * @param sendable object
+   * @return Subsystem name (empty if object is not in registry)
+   */
+  public static synchronized String getSubsystem(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      return "";
+    }
+    return comp.m_subsystem;
+  }
+
+  /**
+   * Sets the subsystem name of an object.
+   *
+   * @param sendable object
+   * @param subsystem subsystem name
+   */
+  public static synchronized void setSubsystem(Sendable sendable, String subsystem) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_subsystem = subsystem;
+    }
+  }
+
+  /**
+   * Gets a unique handle for setting/getting data with setData() and getData().
+   *
+   * @return Handle
+   */
+  public static synchronized int getDataHandle() {
+    return nextDataHandle++;
+  }
+
+  /**
+   * Associates arbitrary data with an object in the registry.
+   *
+   * @param sendable object
+   * @param handle data handle returned by getDataHandle()
+   * @param data data to set
+   * @return Previous data (may be null)
+   */
+  public static synchronized Object setData(Sendable sendable, int handle, Object data) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      return null;
+    }
+    Object rv = null;
+    if (handle < comp.m_data.length) {
+      rv = comp.m_data[handle];
+    } else if (comp.m_data == null) {
+      comp.m_data = new Object[handle + 1];
+    } else {
+      comp.m_data = Arrays.copyOf(comp.m_data, handle + 1);
+    }
+    comp.m_data[handle] = data;
+    return rv;
+  }
+
+  /**
+   * Gets arbitrary data associated with an object in the registry.
+   *
+   * @param sendable object
+   * @param handle data handle returned by getDataHandle()
+   * @return data (may be null if none associated)
+   */
+  public static synchronized Object getData(Sendable sendable, int handle) {
+    Component comp = components.get(sendable);
+    if (comp == null || comp.m_data == null || handle >= comp.m_data.length) {
+      return null;
+    }
+    return comp.m_data[handle];
+  }
+
+  /**
+   * Enables LiveWindow for an object.
+   *
+   * @param sendable object
+   */
+  public static synchronized void enableLiveWindow(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_liveWindow = true;
+    }
+  }
+
+  /**
+   * Disables LiveWindow for an object.
+   *
+   * @param sendable object
+   */
+  public static synchronized void disableLiveWindow(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_liveWindow = false;
+    }
+  }
+
+  /**
+   * Publishes an object in the registry to a network table.
+   *
+   * @param sendable object
+   * @param table network table
+   */
+  public static synchronized void publish(Sendable sendable, NetworkTable table) {
+    Component comp = getOrAdd(sendable);
+    comp.m_builder.clearProperties();
+    comp.m_builder.setTable(table);
+    sendable.initSendable(comp.m_builder);
+    comp.m_builder.updateTable();
+    comp.m_builder.startListeners();
+  }
+
+  /**
+   * Updates network table information from an object.
+   *
+   * @param sendable object
+   */
+  public static synchronized void update(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_builder.updateTable();
+    }
+  }
+
+  /**
+   * Data passed to foreachLiveWindow() callback function.
+   */
+  public static class CallbackData {
+    /**
+     * Sendable object.
+     */
+    @SuppressWarnings("MemberName")
+    public Sendable sendable;
+
+    /**
+     * Name.
+     */
+    @SuppressWarnings("MemberName")
+    public String name;
+
+    /**
+     * Subsystem.
+     */
+    @SuppressWarnings("MemberName")
+    public String subsystem;
+
+    /**
+     * Parent sendable object.
+     */
+    @SuppressWarnings("MemberName")
+    public Sendable parent;
+
+    /**
+     * Data stored in object with setData().  Update this to change the data.
+     */
+    @SuppressWarnings("MemberName")
+    public Object data;
+
+    /**
+     * Sendable builder for the sendable.
+     */
+    @SuppressWarnings("MemberName")
+    public SendableBuilderImpl builder;
+  }
+
+  // As foreachLiveWindow is single threaded, cache the components it
+  // iterates over to avoid risk of ConcurrentModificationException
+  private static List<Component> foreachComponents = new ArrayList<>();
+
+  /**
+   * Iterates over LiveWindow-enabled objects in the registry.
+   * It is *not* safe to call other SendableRegistry functions from the
+   * callback.
+   *
+   * @param dataHandle data handle to get data object passed to callback
+   * @param callback function to call for each object
+   */
+  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
+                     "PMD.AvoidCatchingThrowable"})
+  public static synchronized void foreachLiveWindow(int dataHandle,
+      Consumer<CallbackData> callback) {
+    CallbackData cbdata = new CallbackData();
+    foreachComponents.clear();
+    foreachComponents.addAll(components.values());
+    for (Component comp : foreachComponents) {
+      if (comp.m_sendable == null) {
+        continue;
+      }
+      cbdata.sendable = comp.m_sendable.get();
+      if (cbdata.sendable != null && comp.m_liveWindow) {
+        cbdata.name = comp.m_name;
+        cbdata.subsystem = comp.m_subsystem;
+        if (comp.m_parent != null) {
+          cbdata.parent = comp.m_parent.get();
+        } else {
+          cbdata.parent = null;
+        }
+        if (comp.m_data != null && dataHandle < comp.m_data.length) {
+          cbdata.data = comp.m_data[dataHandle];
+        } else {
+          cbdata.data = null;
+        }
+        cbdata.builder = comp.m_builder;
+        try {
+          callback.accept(cbdata);
+        } catch (Throwable throwable) {
+          Throwable cause = throwable.getCause();
+          if (cause != null) {
+            throwable = cause;
+          }
+          DriverStation.reportError(
+              "Unhandled exception calling LiveWindow for " + comp.m_name + ": "
+                  + throwable.toString(), false);
+          comp.m_liveWindow = false;
+        }
+        if (cbdata.data != null) {
+          if (comp.m_data == null) {
+            comp.m_data = new Object[dataHandle + 1];
+          } else if (dataHandle >= comp.m_data.length) {
+            comp.m_data = Arrays.copyOf(comp.m_data, dataHandle + 1);
+          }
+          comp.m_data[dataHandle] = cbdata.data;
+        }
+      }
+    }
+    foreachComponents.clear();
+  }
+}
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..457ae46
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
@@ -0,0 +1,537 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.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 final class SmartDashboard {
+  /**
+   * The {@link NetworkTable} used by {@link SmartDashboard}.
+   */
+  private static final NetworkTable table =
+      NetworkTableInstance.getDefault().getTable("SmartDashboard");
+
+  /**
+   * A table linking tables in the SmartDashboard to the {@link Sendable} objects they
+   * came from.
+   */
+  @SuppressWarnings("PMD.UseConcurrentHashMap")
+  private static final Map<String, Sendable> tablesToData = new HashMap<>();
+
+  /**
+   * The executor for listener tasks; calls listener tasks synchronously from main thread.
+   */
+  private static final ListenerExecutor listenerExecutor = new ListenerExecutor();
+
+  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
+   */
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public static synchronized void putData(String key, Sendable data) {
+    Sendable sddata = tablesToData.get(key);
+    if (sddata == null || sddata != data) {
+      tablesToData.put(key, data);
+      NetworkTable dataTable = table.getSubTable(key);
+      SendableRegistry.publish(data, dataTable);
+      dataTable.getEntry(".name").setString(key);
+    }
+  }
+
+  /**
+   * Maps the specified key (where the key is the name of the {@link Sendable}
+   * 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) {
+    String name = SendableRegistry.getName(value);
+    if (!name.isEmpty()) {
+      putData(name, 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) {
+    Sendable data = tablesToData.get(key);
+    if (data == null) {
+      throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
+    } else {
+      return data;
+    }
+  }
+
+  /**
+   * 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);
+  }
+
+  /**
+   * Posts a task from a listener to the ListenerExecutor, so that it can be run synchronously
+   * from the main loop on the next call to {@link SmartDashboard#updateValues()}.
+   *
+   * @param task The task to run synchronously from the main thread.
+   */
+  public static void postListenerTask(Runnable task) {
+    listenerExecutor.execute(task);
+  }
+
+  /**
+   * Puts all sendable data to the dashboard.
+   */
+  public static synchronized void updateValues() {
+    for (Sendable data : tablesToData.values()) {
+      SendableRegistry.update(data);
+    }
+    // Execute posted listener tasks
+    listenerExecutor.runListenerTasks();
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
new file mode 100644
index 0000000..578787c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class CubicHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * Constructs a cubic hermite spline with the specified control vectors. Each
+   * control vector contains info about the location of the point and its first
+   * derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   *                              the x dimension.
+   * @param xFinalControlVector   The control vector for the final point in
+   *                              the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   *                              the y dimension.
+   * @param yFinalControlVector   The control vector for the final point in
+   *                              the y dimension.
+   */
+  @SuppressWarnings("ParameterName")
+  public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
+                            double[] yInitialControlVector, double[] yFinalControlVector) {
+    super(3);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 4);
+
+    for (int i = 0; i < 4; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Then populate row 4 and 5 with the second derivatives.
+      // Here, we are multiplying by (3 - i) to manually take the derivative. The
+      // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
+    }
+
+    for (int i = 0; i < 3; i++) {
+      // Here, we are multiplying by (2 - i) to manually take the derivative. The
+      // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
+    }
+
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  private SimpleMatrix makeHermiteBasis() {
+    if (hermiteBasis == null) {
+      hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{
+          +2.0, +1.0, -2.0, +1.0,
+          -3.0, -2.0, +3.0, -1.0,
+          +0.0, +1.0, +0.0, +0.0,
+          +1.0, +0.0, +0.0, +0.0
+      });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector   The control vector for the final point.
+   * @return The control vector matrix for a dimension.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 2 || finalVector.length != 2) {
+      throw new IllegalArgumentException("Size of vectors must be 2");
+    }
+    return new SimpleMatrix(4, 1, true, new double[]{
+        initialVector[0], initialVector[1],
+        finalVector[0], finalVector[1]});
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
new file mode 100644
index 0000000..62c1e5a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.spline;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * Represents a pair of a pose and a curvature.
+ */
+@SuppressWarnings("MemberName")
+public class PoseWithCurvature {
+  // Represents the pose.
+  public Pose2d poseMeters;
+
+  // Represents the curvature.
+  public double curvatureRadPerMeter;
+
+  /**
+   * Constructs a PoseWithCurvature.
+   *
+   * @param poseMeters           The pose.
+   * @param curvatureRadPerMeter The curvature.
+   */
+  public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
+    this.poseMeters = poseMeters;
+    this.curvatureRadPerMeter = curvatureRadPerMeter;
+  }
+
+  /**
+   * Constructs a PoseWithCurvature with default values.
+   */
+  public PoseWithCurvature() {
+    poseMeters = new Pose2d();
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
new file mode 100644
index 0000000..3b2d3eb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class QuinticHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * Constructs a quintic hermite spline with the specified control vectors.
+   * Each control vector contains into about the location of the point, its
+   * first derivative, and its second derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   *                              the x dimension.
+   * @param xFinalControlVector   The control vector for the final point in
+   *                              the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   *                              the y dimension.
+   * @param yFinalControlVector   The control vector for the final point in
+   *                              the y dimension.
+   */
+  @SuppressWarnings("ParameterName")
+  public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
+                              double[] yInitialControlVector, double[] yFinalControlVector) {
+    super(5);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 6);
+
+    for (int i = 0; i < 6; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+    }
+    for (int i = 0; i < 6; i++) {
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Here, we are multiplying by (5 - i) to manually take the derivative. The
+      // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
+    }
+    for (int i = 0; i < 5; i++) {
+      // Then populate row 4 and 5 with the second derivatives.
+      // Here, we are multiplying by (4 - i) to manually take the derivative. The
+      // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  private SimpleMatrix makeHermiteBasis() {
+    if (hermiteBasis == null) {
+      hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{
+          -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+          +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+          -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+          +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+          +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+          +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
+      });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector   The control vector for the final point.
+   * @return The control vector matrix for a dimension.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 3 || finalVector.length != 3) {
+      throw new IllegalArgumentException("Size of vectors must be 3");
+    }
+    return new SimpleMatrix(6, 1, true, new double[]{
+        initialVector[0], initialVector[1], initialVector[2],
+        finalVector[0], finalVector[1], finalVector[2]});
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
new file mode 100644
index 0000000..a0dfd19
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.spline;
+
+import java.util.Arrays;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents a two-dimensional parametric spline that interpolates between two
+ * points.
+ */
+public abstract class Spline {
+  private final int m_degree;
+
+  /**
+   * Constructs a spline with the given degree.
+   *
+   * @param degree The degree of the spline.
+   */
+  Spline(int degree) {
+    m_degree = degree;
+  }
+
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  protected abstract SimpleMatrix getCoefficients();
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @param t The point t
+   * @return The pose and curvature at that point.
+   */
+  @SuppressWarnings("ParameterName")
+  public PoseWithCurvature getPoint(double t) {
+    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
+    final var coefficients = getCoefficients();
+
+    // Populate the polynomial bases.
+    for (int i = 0; i <= m_degree; i++) {
+      polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
+    }
+
+    // This simply multiplies by the coefficients. We need to divide out t some
+    // n number of times where n is the derivative we want to take.
+    SimpleMatrix combined = coefficients.mult(polynomialBases);
+
+    // Get x and y
+    final double x = combined.get(0, 0);
+    final double y = combined.get(1, 0);
+
+    double dx;
+    double dy;
+    double ddx;
+    double ddy;
+
+    if (t == 0) {
+      dx = coefficients.get(2, m_degree - 1);
+      dy = coefficients.get(3, m_degree - 1);
+      ddx = coefficients.get(4, m_degree - 2);
+      ddy = coefficients.get(5, m_degree - 2);
+    } else {
+      // Divide out t once for first derivative.
+      dx = combined.get(2, 0) / t;
+      dy = combined.get(3, 0) / t;
+
+      // Divide out t twice for second derivative.
+      ddx = combined.get(4, 0) / t / t;
+      ddy = combined.get(5, 0) / t / t;
+    }
+
+    // Find the curvature.
+    final double curvature =
+        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
+
+    return new PoseWithCurvature(
+        new Pose2d(x, y, new Rotation2d(dx, dy)),
+        curvature
+    );
+  }
+
+  /**
+   * Represents a control vector for a spline.
+   *
+   * <p>Each element in each array represents the value of the derivative at the index. For
+   * example, the value of x[2] is the second derivative in the x dimension.
+   */
+  @SuppressWarnings("MemberName")
+  public static class ControlVector {
+    public double[] x;
+    public double[] y;
+
+    /**
+     * Instantiates a control vector.
+     * @param x The x dimension of the control vector.
+     * @param y The y dimension of the control vector.
+     */
+    @SuppressWarnings("ParameterName")
+    public ControlVector(double[] x, double[] y) {
+      this.x = Arrays.copyOf(x, x.length);
+      this.y = Arrays.copyOf(y, y.length);
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
new file mode 100644
index 0000000..a419bf9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
@@ -0,0 +1,276 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.spline;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+public final class SplineHelper {
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private SplineHelper() {
+  }
+
+  /**
+   * Returns 2 cubic control vectors from a set of exterior waypoints and
+   * interior translations.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @return 2 cubic control vectors.
+   */
+  public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
+      Pose2d start, Translation2d[] interiorWaypoints, Pose2d end
+  ) {
+    // Generate control vectors from poses.
+    Spline.ControlVector initialCV;
+    Spline.ControlVector endCV;
+
+    // Chooses a magnitude automatically that makes the splines look better.
+    if (interiorWaypoints.length < 1) {
+      double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      endCV = getCubicControlVector(scalar, end);
+    } else {
+      double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1])
+          * 1.2;
+      endCV = getCubicControlVector(scalar, end);
+    }
+    return new Spline.ControlVector[]{initialCV, endCV};
+  }
+
+  /**
+   * Returns quintic control vectors from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of control vectors
+   */
+  public static List<Spline.ControlVector> getQuinticControlVectorsFromWaypoints(
+      List<Pose2d> waypoints
+  ) {
+    List<Spline.ControlVector> vectors = new ArrayList<>();
+    for (int i = 0; i < waypoints.size() - 1; i++) {
+      var p0 = waypoints.get(i);
+      var p1 = waypoints.get(i + 1);
+
+      // This just makes the splines look better.
+      final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
+
+      vectors.add(getQuinticControlVector(scalar, p0));
+      vectors.add(getQuinticControlVector(scalar, p1));
+    }
+    return vectors;
+  }
+
+  /**
+   * Returns a set of cubic splines corresponding to the provided control vectors. The
+   * user is free to set the direction of the start and end point. The
+   * directions for the middle waypoints are determined automatically to ensure
+   * continuous curvature throughout the path.
+   *
+   * @param start     The starting control vector.
+   * @param waypoints The middle waypoints. This can be left blank if you only
+   *                  wish to create a path with two waypoints.
+   * @param end       The ending control vector.
+   * @return A vector of cubic hermite splines that interpolate through the
+   *         provided waypoints and control vectors.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
+                        "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
+      Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
+
+    CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
+
+    double[] xInitial = start.x;
+    double[] yInitial = start.y;
+    double[] xFinal = end.x;
+    double[] yFinal = end.y;
+
+    if (waypoints.length > 1) {
+      Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
+
+      // Create an array of all waypoints, including the start and end.
+      newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
+      System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
+      newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
+
+      // Populate tridiagonal system for clamped cubic
+      /* See:
+      https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+      /undervisningsmateriale/chap7alecture.pdf
+      */
+      // Above-diagonal of tridiagonal matrix, zero-padded
+      final double[] a = new double[newWaypts.length - 2];
+
+      // Diagonal of tridiagonal matrix
+      final double[] b = new double[newWaypts.length - 2];
+      Arrays.fill(b, 4.0);
+
+      // Below-diagonal of tridiagonal matrix, zero-padded
+      final double[] c = new double[newWaypts.length - 2];
+
+      // rhs vectors
+      final double[] dx = new double[newWaypts.length - 2];
+      final double[] dy = new double[newWaypts.length - 2];
+
+      // solution vectors
+      final double[] fx = new double[newWaypts.length - 2];
+      final double[] fy = new double[newWaypts.length - 2];
+
+      // populate above-diagonal and below-diagonal vectors
+      a[0] = 0.0;
+      for (int i = 0; i < newWaypts.length - 3; i++) {
+        a[i + 1] = 1;
+        c[i] = 1;
+      }
+      c[c.length - 1] = 0.0;
+
+      // populate rhs vectors
+      dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
+      dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
+
+      if (newWaypts.length > 4) {
+        for (int i = 1; i <= newWaypts.length - 4; i++) {
+          dx[i] = 3 * (newWaypts[i + 1].getX() - newWaypts[i - 1].getX());
+          dy[i] = 3 * (newWaypts[i + 1].getY() - newWaypts[i - 1].getY());
+        }
+      }
+
+      dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
+          - newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
+      dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
+          - newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
+
+      // Compute solution to tridiagonal system
+      thomasAlgorithm(a, b, c, dx, fx);
+      thomasAlgorithm(a, b, c, dy, fy);
+
+      double[] newFx = new double[fx.length + 2];
+      double[] newFy = new double[fy.length + 2];
+
+      newFx[0] = xInitial[1];
+      newFy[0] = yInitial[1];
+      System.arraycopy(fx, 0, newFx, 1, fx.length);
+      System.arraycopy(fy, 0, newFy, 1, fy.length);
+      newFx[newFx.length - 1] = xFinal[1];
+      newFy[newFy.length - 1] = yFinal[1];
+
+      for (int i = 0; i < newFx.length - 1; i++) {
+        splines[i] = new CubicHermiteSpline(
+            new double[]{newWaypts[i].getX(), newFx[i]},
+            new double[]{newWaypts[i + 1].getX(), newFx[i + 1]},
+            new double[]{newWaypts[i].getY(), newFy[i]},
+            new double[]{newWaypts[i + 1].getY(), newFy[i + 1]}
+        );
+      }
+    } else if (waypoints.length == 1) {
+      final var xDeriv = (3 * (xFinal[0]
+          - xInitial[0])
+          - xFinal[1] - xInitial[1])
+          / 4.0;
+      final var yDeriv = (3 * (yFinal[0]
+          - yInitial[0])
+          - yFinal[1] - yInitial[1])
+          / 4.0;
+
+      double[] midXControlVector = {waypoints[0].getX(), xDeriv};
+      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
+
+      splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
+                                          yInitial, midYControlVector);
+      splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
+                                          midYControlVector, yFinal);
+    } else {
+      splines[0] = new CubicHermiteSpline(xInitial, xFinal,
+                                          yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided control vectors.
+   * The user is free to set the direction of all control vectors. Continuous
+   * curvature is guaranteed throughout the path.
+   *
+   * @param controlVectors The control vectors.
+   * @return A vector of quintic hermite splines that interpolate through the
+   *         provided waypoints.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
+      Spline.ControlVector[] controlVectors) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+    for (int i = 0; i < controlVectors.length - 1; i++) {
+      var xInitial = controlVectors[i].x;
+      var xFinal = controlVectors[i + 1].x;
+      var yInitial = controlVectors[i].y;
+      var yFinal = controlVectors[i + 1].y;
+      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
+                                            yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Thomas algorithm for solving tridiagonal systems Af = d.
+   *
+   * @param a              the values of A above the diagonal
+   * @param b              the values of A on the diagonal
+   * @param c              the values of A below the diagonal
+   * @param d              the vector on the rhs
+   * @param solutionVector the unknown (solution) vector, modified in-place
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  private static void thomasAlgorithm(double[] a, double[] b,
+                                      double[] c, double[] d, double[] solutionVector) {
+    int N = d.length;
+
+    double[] cStar = new double[N];
+    double[] dStar = new double[N];
+
+    // This updates the coefficients in the first row
+    // Note that we should be checking for division by zero here
+    cStar[0] = c[0] / b[0];
+    dStar[0] = d[0] / b[0];
+
+    // Create the c_star and d_star coefficients in the forward sweep
+    for (int i = 1; i < N; i++) {
+      double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
+      cStar[i] = c[i] * m;
+      dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
+    }
+    solutionVector[N - 1] = dStar[N - 1];
+    // This is the reverse sweep, used to update the solution vector f
+    for (int i = N - 2; i >= 0; i--) {
+      solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
+    }
+  }
+
+  private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos()},
+        new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()}
+    );
+  }
+
+  private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0},
+        new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0}
+    );
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
new file mode 100644
index 0000000..4b7bf95
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+public final class SplineParameterizer {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  /**
+   * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
+   * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
+   * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
+   * this maximum should definitely indicate something has gone wrong.
+   */
+  private static final int kMaxIterations = 5000;
+
+  @SuppressWarnings("MemberName")
+  private static class StackContents {
+    final double t1;
+    final double t0;
+
+    StackContents(double t0, double t1) {
+      this.t0 = t0;
+      this.t1 = t1;
+    }
+  }
+
+  public static class MalformedSplineException extends RuntimeException {
+    /**
+     * Create a new exception with the given message.
+     *
+     * @param message the message to pass with the exception
+     */
+    private MalformedSplineException(String message) {
+      super(message);
+    }
+  }
+
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private SplineParameterizer() {
+  }
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> parameterize(Spline spline) {
+    return parameterize(spline, 0.0, 1.0);
+  }
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @param t0     Starting internal spline parameter. It is recommended to use 0.0.
+   * @param t1     Ending internal spline parameter. It is recommended to use 1.0.
+   * @return       A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.add(spline.getPoint(t0));
+
+    // We use an "explicit stack" to simulate recursion, instead of a recursive function call
+    // This give us greater control, instead of a stack overflow
+    var stack = new ArrayDeque<StackContents>();
+    stack.push(new StackContents(t0, t1));
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.isEmpty()) {
+      current = stack.removeFirst();
+      start = spline.getPoint(current.t0);
+      end = spline.getPoint(current.t1);
+
+      final var twist = start.poseMeters.log(end.poseMeters);
+      if (
+          Math.abs(twist.dy) > kMaxDy
+          || Math.abs(twist.dx) > kMaxDx
+          || Math.abs(twist.dtheta) > kMaxDtheta
+      ) {
+        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+      } else {
+        splinePoints.add(spline.getPoint(current.t1));
+      }
+
+      iterations++;
+      if (iterations >= kMaxIterations) {
+        throw new MalformedSplineException(
+          "Could not parameterize a malformed spline. "
+          + "This means that you probably had two or more adjacent waypoints that were very close "
+          + "together with headings in opposing directions."
+        );
+      }
+    }
+
+    return splinePoints;
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
new file mode 100644
index 0000000..f1d51b9
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
@@ -0,0 +1,331 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+public class Trajectory {
+  private final double m_totalTimeSeconds;
+  private final List<State> m_states;
+
+  /**
+   * Constructs a trajectory from a vector of states.
+   *
+   * @param states A vector of states.
+   */
+  public Trajectory(final List<State> states) {
+    m_states = states;
+    m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
+  }
+
+  /**
+   * Linearly interpolates between two values.
+   *
+   * @param startValue The start value.
+   * @param endValue   The end value.
+   * @param t          The fraction for interpolation.
+   * @return The interpolated value.
+   */
+  @SuppressWarnings("ParameterName")
+  private static double lerp(double startValue, double endValue, double t) {
+    return startValue + (endValue - startValue) * t;
+  }
+
+  /**
+   * Linearly interpolates between two poses.
+   *
+   * @param startValue The start pose.
+   * @param endValue   The end pose.
+   * @param t          The fraction for interpolation.
+   * @return The interpolated pose.
+   */
+  @SuppressWarnings("ParameterName")
+  private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
+    return startValue.plus((endValue.minus(startValue)).times(t));
+  }
+
+  /**
+   * Returns the initial pose of the trajectory.
+   *
+   * @return The initial pose of the trajectory.
+   */
+  public Pose2d getInitialPose() {
+    return sample(0).poseMeters;
+  }
+
+  /**
+   * Returns the overall duration of the trajectory.
+   *
+   * @return The duration of the trajectory.
+   */
+  public double getTotalTimeSeconds() {
+    return m_totalTimeSeconds;
+  }
+
+  /**
+   * Return the states of the trajectory.
+   *
+   * @return The states of the trajectory.
+   */
+  public List<State> getStates() {
+    return m_states;
+  }
+
+  /**
+   * Sample the trajectory at a point in time.
+   *
+   * @param timeSeconds The point in time since the beginning of the trajectory to sample.
+   * @return The state at that point in time.
+   */
+  public State sample(double timeSeconds) {
+    if (timeSeconds <= m_states.get(0).timeSeconds) {
+      return m_states.get(0);
+    }
+    if (timeSeconds >= m_totalTimeSeconds) {
+      return m_states.get(m_states.size() - 1);
+    }
+
+    // To get the element that we want, we will use a binary search algorithm
+    // instead of iterating over a for-loop. A binary search is O(std::log(n))
+    // whereas searching using a loop is O(n).
+
+    // This starts at 1 because we use the previous state later on for
+    // interpolation.
+    int low = 1;
+    int high = m_states.size() - 1;
+
+    while (low != high) {
+      int mid = (low + high) / 2;
+      if (m_states.get(mid).timeSeconds < timeSeconds) {
+        // This index and everything under it are less than the requested
+        // timestamp. Therefore, we can discard them.
+        low = mid + 1;
+      } else {
+        // t is at least as large as the element at this index. This means that
+        // anything after it cannot be what we are looking for.
+        high = mid;
+      }
+    }
+
+    // High and Low should be the same.
+
+    // The sample's timestamp is now greater than or equal to the requested
+    // timestamp. If it is greater, we need to interpolate between the
+    // previous state and the current state to get the exact state that we
+    // want.
+    final State sample = m_states.get(low);
+    final State prevSample = m_states.get(low - 1);
+
+    // If the difference in states is negligible, then we are spot on!
+    if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
+      return sample;
+    }
+    // Interpolate between the two states for the state that we want.
+    return prevSample.interpolate(sample,
+        (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
+  }
+
+  /**
+   * Transforms all poses in the trajectory by the given transform. This is
+   * useful for converting a robot-relative trajectory into a field-relative
+   * trajectory. This works with respect to the first pose in the trajectory.
+   *
+   * @param transform The transform to transform the trajectory by.
+   * @return The transformed trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public Trajectory transformBy(Transform2d transform) {
+    var firstState = m_states.get(0);
+    var firstPose = firstState.poseMeters;
+
+    // Calculate the transformed first pose.
+    var newFirstPose = firstPose.plus(transform);
+    List<State> newStates = new ArrayList<>();
+
+    newStates.add(new State(
+        firstState.timeSeconds, firstState.velocityMetersPerSecond,
+        firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
+    ));
+
+    for (int i = 1; i < m_states.size(); i++) {
+      var state = m_states.get(i);
+      // We are transforming relative to the coordinate frame of the new initial pose.
+      newStates.add(new State(
+          state.timeSeconds, state.velocityMetersPerSecond,
+          state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
+          state.curvatureRadPerMeter
+      ));
+    }
+
+    return new Trajectory(newStates);
+  }
+
+  /**
+   * Transforms all poses in the trajectory so that they are relative to the
+   * given pose. This is useful for converting a field-relative trajectory
+   * into a robot-relative trajectory.
+   *
+   * @param pose The pose that is the origin of the coordinate frame that
+   *             the current trajectory will be transformed into.
+   * @return The transformed trajectory.
+   */
+  public Trajectory relativeTo(Pose2d pose) {
+    return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
+        state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
+        state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
+        .collect(Collectors.toList()));
+  }
+
+  /**
+   * Represents a time-parameterized trajectory. The trajectory contains of
+   * various States that represent the pose, curvature, time elapsed, velocity,
+   * and acceleration at that point.
+   */
+  @SuppressWarnings("MemberName")
+  public static class State {
+    // The time elapsed since the beginning of the trajectory.
+    @JsonProperty("time")
+    public double timeSeconds;
+
+    // The speed at that point of the trajectory.
+    @JsonProperty("velocity")
+    public double velocityMetersPerSecond;
+
+    // The acceleration at that point of the trajectory.
+    @JsonProperty("acceleration")
+    public double accelerationMetersPerSecondSq;
+
+    // The pose at that point of the trajectory.
+    @JsonProperty("pose")
+    public Pose2d poseMeters;
+
+    // The curvature at that point of the trajectory.
+    @JsonProperty("curvature")
+    public double curvatureRadPerMeter;
+
+    public State() {
+      poseMeters = new Pose2d();
+    }
+
+    /**
+     * Constructs a State with the specified parameters.
+     *
+     * @param timeSeconds                   The time elapsed since the beginning of the trajectory.
+     * @param velocityMetersPerSecond       The speed at that point of the trajectory.
+     * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
+     * @param poseMeters                    The pose at that point of the trajectory.
+     * @param curvatureRadPerMeter          The curvature at that point of the trajectory.
+     */
+    public State(double timeSeconds, double velocityMetersPerSecond,
+                 double accelerationMetersPerSecondSq, Pose2d poseMeters,
+                 double curvatureRadPerMeter) {
+      this.timeSeconds = timeSeconds;
+      this.velocityMetersPerSecond = velocityMetersPerSecond;
+      this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
+      this.poseMeters = poseMeters;
+      this.curvatureRadPerMeter = curvatureRadPerMeter;
+    }
+
+    /**
+     * Interpolates between two States.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i        The interpolant (fraction).
+     * @return The interpolated state.
+     */
+    @SuppressWarnings("ParameterName")
+    State interpolate(State endValue, double i) {
+      // Find the new t value.
+      final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
+
+      // Find the delta time between the current state and the interpolated state.
+      final double deltaT = newT - timeSeconds;
+
+      // If delta time is negative, flip the order of interpolation.
+      if (deltaT < 0) {
+        return endValue.interpolate(this, 1 - i);
+      }
+
+      // Check whether the robot is reversing at this stage.
+      final boolean reversing = velocityMetersPerSecond < 0
+          || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
+
+      // Calculate the new velocity
+      // v_f = v_0 + at
+      final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
+
+      // Calculate the change in position.
+      // delta_s = v_0 t + 0.5 at^2
+      final double newS = (velocityMetersPerSecond * deltaT
+          + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
+
+      // Return the new state. To find the new position for the new state, we need
+      // to interpolate between the two endpoint poses. The fraction for
+      // interpolation is the change in position (delta s) divided by the total
+      // distance between the two endpoints.
+      final double interpolationFrac = newS
+          / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
+
+      return new State(
+          newT, newV, accelerationMetersPerSecondSq,
+          lerp(poseMeters, endValue.poseMeters, interpolationFrac),
+          lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
+      );
+    }
+
+    @Override
+    public String toString() {
+      return String.format(
+        "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+        timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
+        poseMeters, curvatureRadPerMeter);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (this == obj) {
+        return true;
+      }
+      if (!(obj instanceof State)) {
+        return false;
+      }
+      State state = (State) obj;
+      return Double.compare(state.timeSeconds, timeSeconds) == 0
+              && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
+              && Double.compare(state.accelerationMetersPerSecondSq,
+                accelerationMetersPerSecondSq) == 0
+              && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
+              && Objects.equals(poseMeters, state.poseMeters);
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(timeSeconds, velocityMetersPerSecond,
+              accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
+    }
+  }
+
+  @Override
+  public String toString() {
+    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
new file mode 100644
index 0000000..fbc5ddb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+/**
+ * Represents the configuration for generating a trajectory. This class stores the start velocity,
+ * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
+ *
+ * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
+ * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable
+ * values (0, 0, {}, false). These values can be changed via the setXXX methods.
+ */
+public class TrajectoryConfig {
+  private final double m_maxVelocity;
+  private final double m_maxAcceleration;
+  private final List<TrajectoryConstraint> m_constraints;
+  private double m_startVelocity;
+  private double m_endVelocity;
+  private boolean m_reversed;
+
+  /**
+   * Constructs the trajectory configuration class.
+   *
+   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   */
+  public TrajectoryConfig(double maxVelocityMetersPerSecond,
+                          double maxAccelerationMetersPerSecondSq) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
+    m_constraints = new ArrayList<>();
+  }
+
+  /**
+   * Adds a user-defined constraint to the trajectory.
+   *
+   * @param constraint The user-defined constraint.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
+    m_constraints.add(constraint);
+    return this;
+  }
+
+  /**
+   * Adds all user-defined constraints from a list to the trajectory.
+   * @param constraints List of user-defined constraints.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
+    m_constraints.addAll(constraints);
+    return this;
+  }
+
+  /**
+   * Adds a differential drive kinematics constraint to ensure that
+   * no wheel velocity of a differential drive goes above the max velocity.
+   *
+   * @param kinematics The differential drive kinematics.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
+    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+  * Adds a mecanum drive kinematics constraint to ensure that
+  * no wheel velocity of a mecanum drive goes above the max velocity.
+  *
+  * @param kinematics The mecanum drive kinematics.
+  * @return Instance of the current config object.
+  */
+  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
+    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+  * Adds a swerve drive kinematics constraint to ensure that
+  * no wheel velocity of a swerve drive goes above the max velocity.
+  *
+  * @param kinematics The swerve drive kinematics.
+  * @return Instance of the current config object.
+  */
+  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
+    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+  * Returns the starting velocity of the trajectory.
+  *
+  * @return The starting velocity of the trajectory.
+  */
+  public double getStartVelocity() {
+    return m_startVelocity;
+  }
+
+  /**
+   * Sets the start velocity of the trajectory.
+   *
+   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
+    m_startVelocity = startVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   *
+   * @return The starting velocity of the trajectory.
+   */
+  public double getEndVelocity() {
+    return m_endVelocity;
+  }
+
+  /**
+   * Sets the end velocity of the trajectory.
+   *
+   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
+    m_endVelocity = endVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the maximum velocity of the trajectory.
+   *
+   * @return The maximum velocity of the trajectory.
+   */
+  public double getMaxVelocity() {
+    return m_maxVelocity;
+  }
+
+  /**
+   * Returns the maximum acceleration of the trajectory.
+   *
+   * @return The maximum acceleration of the trajectory.
+   */
+  public double getMaxAcceleration() {
+    return m_maxAcceleration;
+  }
+
+  /**
+   * Returns the user-defined constraints of the trajectory.
+   *
+   * @return The user-defined constraints of the trajectory.
+   */
+  public List<TrajectoryConstraint> getConstraints() {
+    return m_constraints;
+  }
+
+  /**
+   * Returns whether the trajectory is reversed or not.
+   *
+   * @return whether the trajectory is reversed or not.
+   */
+  public boolean isReversed() {
+    return m_reversed;
+  }
+
+  /**
+   * Sets the reversed flag of the trajectory.
+   *
+   * @param reversed Whether the trajectory should be reversed or not.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setReversed(boolean reversed) {
+    m_reversed = reversed;
+    return this;
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
new file mode 100644
index 0000000..2e2ae7d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -0,0 +1,229 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.trajectory;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
+import edu.wpi.first.wpilibj.spline.Spline;
+import edu.wpi.first.wpilibj.spline.SplineHelper;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+public final class TrajectoryGenerator {
+  private static final Trajectory kDoNothingTrajectory =
+      new Trajectory(Arrays.asList(new Trajectory.State()));
+
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private TrajectoryGenerator() {
+  }
+
+  /**
+   * Generates a trajectory from the given control vectors and config. This method uses clamped
+   * cubic splines -- a method in which the exterior control vectors and interior waypoints
+   * are provided. The headings are automatically determined at the interior points to
+   * ensure continuous curvature.
+   *
+   * @param initial           The initial control vector.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending control vector.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  public static Trajectory generateTrajectory(
+      Spline.ControlVector initial,
+      List<Translation2d> interiorWaypoints,
+      Spline.ControlVector end,
+      TrajectoryConfig config
+  ) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    // Clone the control vectors.
+    var newInitial = new Spline.ControlVector(initial.x, initial.y);
+    var newEnd = new Spline.ControlVector(end.x, end.y);
+
+    // Change the orientation if reversed.
+    if (config.isReversed()) {
+      newInitial.x[1] *= -1;
+      newInitial.y[1] *= -1;
+      newEnd.x[1] *= -1;
+      newEnd.y[1] *= -1;
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
+          interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+    } catch (MalformedSplineException ex) {
+      DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
+  }
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method uses clamped
+   * cubic splines -- a method in which the initial pose, final pose, and interior waypoints
+   * are provided.  The headings are automatically determined at the interior points to
+   * ensure continuous curvature.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  public static Trajectory generateTrajectory(
+      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
+      TrajectoryConfig config
+  ) {
+    var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
+        start, interiorWaypoints.toArray(new Translation2d[0]), end
+    );
+
+    // Return the generated trajectory.
+    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
+  }
+
+  /**
+   * Generates a trajectory from the given quintic control vectors and config. This method
+   * uses quintic hermite splines -- therefore, all points must be represented by control
+   * vectors. Continuous curvature is guaranteed in this method.
+   *
+   * @param controlVectors List of quintic control vectors.
+   * @param config         The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static Trajectory generateTrajectory(
+      ControlVectorList controlVectors,
+      TrajectoryConfig config
+  ) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
+
+    // Create a new control vector list, flipping the orientation if reversed.
+    for (final var vector : controlVectors) {
+      var newVector = new Spline.ControlVector(vector.x, vector.y);
+      if (config.isReversed()) {
+        newVector.x[1] *= -1;
+        newVector.y[1] *= -1;
+      }
+      newControlVectors.add(newVector);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
+          newControlVectors.toArray(new Spline.ControlVector[]{})
+      ));
+    } catch (MalformedSplineException ex) {
+      DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
+
+  }
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method
+   * uses quintic hermite splines -- therefore, all points must be represented by Pose2d
+   * objects. Continuous curvature is guaranteed in this method.
+   *
+   * @param waypoints List of waypoints..
+   * @param config    The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
+    var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
+    var newList = new ControlVectorList(originalList);
+    return generateTrajectory(newList, config);
+  }
+
+  /**
+   * Generate spline points from a vector of splines by parameterizing the
+   * splines.
+   *
+   * @param splines The splines to parameterize.
+   * @return The spline points for use in time parameterization of a trajectory.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> splinePointsFromSplines(
+      Spline[] splines) {
+    // Create the vector of spline points.
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // Add the first point to the vector.
+    splinePoints.add(splines[0].getPoint(0.0));
+
+    // Iterate through the vector and parameterize each spline, adding the
+    // parameterized points to the final vector.
+    for (final var spline : splines) {
+      var points = SplineParameterizer.parameterize(spline);
+
+      // Append the array of poses to the vector. We are removing the first
+      // point because it's a duplicate of the last point from the previous
+      // spline.
+      splinePoints.addAll(points.subList(1, points.size()));
+    }
+    return splinePoints;
+  }
+
+  // Work around type erasure signatures
+  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
+    public ControlVectorList(int initialCapacity) {
+      super(initialCapacity);
+    }
+
+    public ControlVectorList() {
+      super();
+    }
+
+    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
+      super(collection);
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
new file mode 100644
index 0000000..9459dd0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
@@ -0,0 +1,303 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+/**
+ * Class used to parameterize a trajectory by time.
+ */
+public final class TrajectoryParameterizer {
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private TrajectoryParameterizer() {
+  }
+
+  /**
+   * Parameterize the trajectory by time. This is where the velocity profile is
+   * generated.
+   *
+   * <p>The derivation of the algorithm used can be found
+   * <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
+   * here</a>.
+   *
+   * @param points                           Reference to the spline points.
+   * @param constraints                      A vector of various velocity and acceleration.
+   *                                         constraints.
+   * @param startVelocityMetersPerSecond     The start velocity for the trajectory.
+   * @param endVelocityMetersPerSecond       The end velocity for the trajectory.
+   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   * @param reversed                         Whether the robot should move backwards.
+   *                                         Note that the robot will still move from
+   *                                         a -&gt; b -&gt; ... -&gt; z as defined in the
+   *                                         waypoints.
+   * @return The trajectory.
+   */
+  @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
+      "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
+      "PMD.AvoidThrowingRawExceptionTypes"})
+  public static Trajectory timeParameterizeTrajectory(
+      List<PoseWithCurvature> points,
+      List<TrajectoryConstraint> constraints,
+      double startVelocityMetersPerSecond,
+      double endVelocityMetersPerSecond,
+      double maxVelocityMetersPerSecond,
+      double maxAccelerationMetersPerSecondSq,
+      boolean reversed
+  ) {
+    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
+    var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
+        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
+
+    // Forward pass
+    for (int i = 0; i < points.size(); i++) {
+      constrainedStates.add(new ConstrainedState());
+      var constrainedState = constrainedStates.get(i);
+      constrainedState.pose = points.get(i);
+
+      // Begin constraining based on predecessor.
+      double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
+          predecessor.pose.poseMeters.getTranslation());
+      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
+
+      // We may need to iterate to find the maximum end velocity and common
+      // acceleration, since acceleration limits may be a function of velocity.
+      while (true) {
+        // Enforce global max velocity and max reachable velocity by global
+        // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+        constrainedState.maxVelocityMetersPerSecond = Math.min(
+            maxVelocityMetersPerSecond,
+            Math.sqrt(predecessor.maxVelocityMetersPerSecond
+                * predecessor.maxVelocityMetersPerSecond
+                + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
+        );
+
+        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
+        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+
+        // At this point, the constrained state is fully constructed apart from
+        // all the custom-defined user constraints.
+        for (final var constraint : constraints) {
+          constrainedState.maxVelocityMetersPerSecond = Math.min(
+              constrainedState.maxVelocityMetersPerSecond,
+              constraint.getMaxVelocityMetersPerSecond(
+                  constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
+                  constrainedState.maxVelocityMetersPerSecond)
+          );
+        }
+
+        // Now enforce all acceleration limits.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds < 1E-6) {
+          break;
+        }
+
+        // If the actual acceleration for this state is higher than the max
+        // acceleration that we applied, then we need to reduce the max
+        // acceleration of the predecessor and try again.
+        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
+            * constrainedState.maxVelocityMetersPerSecond
+            - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
+            / (ds * 2.0);
+
+        // If we violate the max acceleration constraint, let's modify the
+        // predecessor.
+        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
+          predecessor.maxAccelerationMetersPerSecondSq
+              = constrainedState.maxAccelerationMetersPerSecondSq;
+        } else {
+          // Constrain the predecessor's max acceleration to the current
+          // acceleration.
+          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
+            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
+          }
+          // If the actual acceleration is less than the predecessor's min
+          // acceleration, it will be repaired in the backward pass.
+          break;
+        }
+      }
+      predecessor = constrainedState;
+    }
+
+    var successor = new ConstrainedState(points.get(points.size() - 1),
+        constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
+        endVelocityMetersPerSecond,
+        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
+
+    // Backward pass
+    for (int i = points.size() - 1; i >= 0; i--) {
+      var constrainedState = constrainedStates.get(i);
+      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
+
+      while (true) {
+        // Enforce max velocity limit (reverse)
+        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+        double newMaxVelocity = Math.sqrt(
+            successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+                + successor.minAccelerationMetersPerSecondSq * ds * 2.0
+        );
+
+        // No more limits to impose! This state can be finalized.
+        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
+          break;
+        }
+
+        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
+
+        // Check all acceleration constraints with the new max velocity.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds > -1E-6) {
+          break;
+        }
+
+        // If the actual acceleration for this state is lower than the min
+        // acceleration, then we need to lower the min acceleration of the
+        // successor and try again.
+        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
+            * constrainedState.maxVelocityMetersPerSecond
+            - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
+            / (ds * 2.0);
+
+        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
+          successor.minAccelerationMetersPerSecondSq
+              = constrainedState.minAccelerationMetersPerSecondSq;
+        } else {
+          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
+          break;
+        }
+      }
+      successor = constrainedState;
+    }
+
+    // Now we can integrate the constrained states forward in time to obtain our
+    // trajectory states.
+    var states = new ArrayList<Trajectory.State>(points.size());
+    double timeSeconds = 0.0;
+    double distanceMeters = 0.0;
+    double velocityMetersPerSecond = 0.0;
+
+    for (int i = 0; i < constrainedStates.size(); i++) {
+      final var state = constrainedStates.get(i);
+
+      // Calculate the change in position between the current state and the previous
+      // state.
+      double ds = state.distanceMeters - distanceMeters;
+
+      // Calculate the acceleration between the current state and the previous
+      // state.
+      double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
+          - velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
+
+      // Calculate dt
+      double dt = 0.0;
+      if (i > 0) {
+        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
+        if (Math.abs(accel) > 1E-6) {
+          // v_f = v_0 + a * t
+          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
+        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
+          // delta_x = v * t
+          dt = ds / velocityMetersPerSecond;
+        } else {
+          throw new RuntimeException("Something went wrong");
+        }
+      }
+
+      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
+      distanceMeters = state.distanceMeters;
+
+      timeSeconds += dt;
+
+      states.add(new Trajectory.State(
+          timeSeconds,
+          reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
+          reversed ? -accel : accel,
+          state.pose.poseMeters, state.pose.curvatureRadPerMeter
+      ));
+    }
+
+    return new Trajectory(states);
+  }
+
+  private static void enforceAccelerationLimits(boolean reverse,
+                                                List<TrajectoryConstraint> constraints,
+                                                ConstrainedState state) {
+
+    for (final var constraint : constraints) {
+      double factor = reverse ? -1.0 : 1.0;
+      final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
+          state.pose.poseMeters, state.pose.curvatureRadPerMeter,
+          state.maxVelocityMetersPerSecond * factor);
+
+      state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
+          reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
+              : minMaxAccel.minAccelerationMetersPerSecondSq);
+
+      state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
+          reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
+              : minMaxAccel.maxAccelerationMetersPerSecondSq);
+    }
+
+  }
+
+  @SuppressWarnings("MemberName")
+  private static class ConstrainedState {
+    PoseWithCurvature pose;
+    double distanceMeters;
+    double maxVelocityMetersPerSecond;
+    double minAccelerationMetersPerSecondSq;
+    double maxAccelerationMetersPerSecondSq;
+
+    ConstrainedState(PoseWithCurvature pose, double distanceMeters,
+                     double maxVelocityMetersPerSecond,
+                     double minAccelerationMetersPerSecondSq,
+                     double maxAccelerationMetersPerSecondSq) {
+      this.pose = pose;
+      this.distanceMeters = distanceMeters;
+      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    ConstrainedState() {
+      pose = new PoseWithCurvature();
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
new file mode 100644
index 0000000..0cd0f49
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.trajectory;
+
+import java.io.BufferedReader;
+import java.io.BufferedWriter;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Arrays;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.ObjectReader;
+import com.fasterxml.jackson.databind.ObjectWriter;
+
+public final class TrajectoryUtil {
+  private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class);
+  private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class);
+
+  private TrajectoryUtil() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Imports a Trajectory from a PathWeaver-style JSON file.
+   * @param path the path of the json file to import from
+   * @return The trajectory represented by the file.
+   * @throws IOException if reading from the file fails
+   */
+  public static Trajectory fromPathweaverJson(Path path) throws IOException {
+    try (BufferedReader reader = Files.newBufferedReader(path)) {
+      Trajectory.State[] state = READER.readValue(reader);
+      return new Trajectory(Arrays.asList(state));
+    }
+  }
+
+  /**
+   * Exports a Trajectory to a PathWeaver-style JSON file.
+   * @param trajectory the trajectory to export
+   * @param path the path of the file to export to
+   * @throws IOException if writing to the file fails
+   */
+  public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
+    Files.createDirectories(path.getParent());
+    try (BufferedWriter writer = Files.newBufferedWriter(path)) {
+      WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
+    }
+  }
+
+  /**
+   * Deserializes a Trajectory from PathWeaver-style JSON.
+   * @param json the string containing the serialized JSON
+   * @return the trajectory represented by the JSON
+   * @throws JsonProcessingException if deserializing the JSON fails
+   */
+  public static Trajectory deserializeTrajectory(String json) throws JsonProcessingException {
+    Trajectory.State[] state = READER.readValue(json);
+    return new Trajectory(Arrays.asList(state));
+  }
+
+  /**
+   * Serializes a Trajectory to PathWeaver-style JSON.
+   * @param trajectory the trajectory to export
+   * @return the string containing the serialized JSON
+   * @throws JsonProcessingException if serializing the Trajectory fails
+   */
+  public static String serializeTrajectory(Trajectory trajectory) throws JsonProcessingException {
+    return WRITER.writeValueAsString(trajectory.getStates().toArray(new Trajectory.State[0]));
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
new file mode 100644
index 0000000..0b24411
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
@@ -0,0 +1,304 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.Objects;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * <p>Initialization:
+ * <pre><code>
+ * TrapezoidProfile.Constraints constraints =
+ *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+ * TrapezoidProfile.State previousProfiledReference =
+ *   new TrapezoidProfile.State(initialReference, 0.0);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ * <pre><code>
+ * TrapezoidProfile profile =
+ *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
+ * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for
+ * `calculate()` and to determine when the profile has completed via
+ * `isFinished()`.
+ */
+public class TrapezoidProfile {
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  private int m_direction;
+
+  private Constraints m_constraints;
+  private State m_initial;
+  private State m_goal;
+
+  private double m_endAccel;
+  private double m_endFullSpeed;
+  private double m_endDeccel;
+
+  public static class Constraints {
+    @SuppressWarnings("MemberName")
+    public double maxVelocity;
+    @SuppressWarnings("MemberName")
+    public double maxAcceleration;
+
+    public Constraints() {
+      HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
+    }
+
+    /**
+     * Construct constraints for a TrapezoidProfile.
+     *
+     * @param maxVelocity maximum velocity
+     * @param maxAcceleration maximum acceleration
+     */
+    public Constraints(double maxVelocity, double maxAcceleration) {
+      this.maxVelocity = maxVelocity;
+      this.maxAcceleration = maxAcceleration;
+      HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
+    }
+  }
+
+  public static class State {
+    @SuppressWarnings("MemberName")
+    public double position;
+    @SuppressWarnings("MemberName")
+    public double velocity;
+
+    public State() {
+    }
+
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   * @param initial     The initial state (usually the current state).
+   */
+  public TrapezoidProfile(Constraints constraints, State goal, State initial) {
+    m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
+    m_constraints = constraints;
+    m_initial = direct(initial);
+    m_goal = direct(goal);
+
+    if (m_initial.velocity > m_constraints.maxVelocity) {
+      m_initial.velocity = m_constraints.maxVelocity;
+    }
+
+    // Deal with a possibly truncated motion profile (with nonzero initial or
+    // final velocity) by calculating the parameters as if the profile began and
+    // ended at zero velocity
+    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+    // Now we can calculate the parameters as if it was a full trapezoid instead
+    // of a truncated one
+
+    double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
+        + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
+        * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   */
+  public TrapezoidProfile(Constraints constraints, State goal) {
+    this(constraints, goal, new State(0, 0));
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  @SuppressWarnings("ParameterName")
+  public State calculate(double t) {
+    State result = m_initial;
+
+    if (t < m_endAccel) {
+      result.velocity += t * m_constraints.maxAcceleration;
+      result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+    } else if (t < m_endFullSpeed) {
+      result.velocity = m_constraints.maxVelocity;
+      result.position += (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration
+          / 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel);
+    } else if (t <= m_endDeccel) {
+      result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+      double timeLeft = m_endDeccel - t;
+      result.position = m_goal.position - (m_goal.velocity + timeLeft
+          * m_constraints.maxAcceleration / 2.0) * timeLeft;
+    } else {
+      result = m_goal;
+    }
+
+    return direct(result);
+  }
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   */
+  public double timeLeftUntil(double target) {
+    double position = m_initial.position * m_direction;
+    double velocity = m_initial.velocity * m_direction;
+
+    double endAccel = m_endAccel * m_direction;
+    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+    if (target < position) {
+      endAccel = -endAccel;
+      endFullSpeed = -endFullSpeed;
+      velocity = -velocity;
+    }
+
+    endAccel = Math.max(endAccel, 0);
+    endFullSpeed = Math.max(endFullSpeed, 0);
+    double endDeccel = m_endDeccel - endAccel - endFullSpeed;
+    endDeccel = Math.max(endDeccel, 0);
+
+    final double acceleration = m_constraints.maxAcceleration;
+    final double decceleration = -m_constraints.maxAcceleration;
+
+    double distToTarget = Math.abs(target - position);
+    if (distToTarget < 1e-6) {
+      return 0;
+    }
+
+    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+    double deccelVelocity;
+    if (endAccel > 0) {
+      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
+    } else {
+      deccelVelocity = velocity;
+    }
+
+    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+    deccelDist = Math.max(deccelDist, 0);
+
+    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+    if (accelDist > distToTarget) {
+      accelDist = distToTarget;
+      fullSpeedDist = 0;
+      deccelDist = 0;
+    } else if (accelDist + fullSpeedDist > distToTarget) {
+      fullSpeedDist = distToTarget - accelDist;
+      deccelDist = 0;
+    } else {
+      deccelDist = distToTarget - fullSpeedDist - accelDist;
+    }
+
+    double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
+        * accelDist))) / acceleration;
+
+    double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
+        + 2 * decceleration * deccelDist))) / decceleration;
+
+    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+    return accelTime + fullSpeedTime + deccelTime;
+  }
+
+  /**
+   * Returns the total time the profile takes to reach the goal.
+   */
+  public double totalTime() {
+    return m_endDeccel;
+  }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * <p>The profile has reached the goal if the time since the profile started
+   * has exceeded the profile's total time.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  @SuppressWarnings("ParameterName")
+  public boolean isFinished(double t) {
+    return t >= totalTime();
+  }
+
+  /**
+   * Returns true if the profile inverted.
+   *
+   * <p>The profile is inverted if goal position is less than the initial position.
+   *
+   * @param initial     The initial state (usually the current state).
+   * @param goal        The desired state when the profile is complete.
+   */
+  private static boolean shouldFlipAcceleration(State initial, State goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  private State direct(State in) {
+    State result = new State(in.position, in.velocity);
+    result.position = result.position * m_direction;
+    result.velocity = result.velocity * m_direction;
+    return result;
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
new file mode 100644
index 0000000..f74e7b4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when
+ * traversing a trajectory. The centripetal acceleration of a robot is defined
+ * as the velocity squared divided by the radius of curvature.
+ *
+ * <p>Effectively, limiting the maximum centripetal acceleration will cause the
+ * robot to slow down around tight turns, making it easier to track trajectories
+ * with sharp turns.
+ */
+public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
+  private final double m_maxCentripetalAccelerationMetersPerSecondSq;
+
+  /**
+   * Constructs a centripetal acceleration constraint.
+   *
+   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
+   */
+  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
+    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // ac = v^2 / r
+    // k (curvature) = 1 / r
+
+    // therefore, ac = v^2 * k
+    // ac / k = v^2
+    // v = std::sqrt(ac / k)
+
+    return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
+        / Math.abs(curvatureRadPerMeter));
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    // The acceleration of the robot has no impact on the centripetal acceleration
+    // of the robot.
+    return new MinMax();
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
new file mode 100644
index 0000000..9a57720
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final DifferentialDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a differential drive dynamics constraint.
+   *
+   * @param kinematics A kinematics component describing the drive geometry.
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
+                                               double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
+        0, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Return the new linear chassis speed.
+    return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
new file mode 100644
index 0000000..71432bf
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A class that enforces constraints on differential drive voltage expenditure based on the motor
+ * dynamics and the drive kinematics.  Ensures that the acceleration of any wheel of the robot
+ * while following the trajectory is never higher than what can be achieved with the given
+ * maximum voltage.
+ */
+public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
+  private final SimpleMotorFeedforward m_feedforward;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final double m_maxVoltage;
+
+  /**
+   * Creates a new DifferentialDriveVoltageConstraint.
+   *
+   * @param feedforward A feedforward component describing the behavior of the drive.
+   * @param kinematics  A kinematics component describing the drive geometry.
+   * @param maxVoltage  The maximum voltage available to the motors while following the path.
+   *                    Should be somewhat less than the nominal battery voltage (12V) to account
+   *                    for "voltage sag" due to current draw.
+   */
+  public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward,
+                                            DifferentialDriveKinematics kinematics,
+                                            double maxVoltage) {
+    m_feedforward = requireNonNullParam(feedforward, "feedforward",
+                                        "DifferentialDriveVoltageConstraint");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics",
+                                       "DifferentialDriveVoltageConstraint");
+    m_maxVoltage = maxVoltage;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    return Double.POSITIVE_INFINITY;
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0,
+                                                                   velocityMetersPerSecond
+                                                                       * curvatureRadPerMeter));
+
+    double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond,
+                                    wheelSpeeds.rightMetersPerSecond);
+    double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond,
+                                    wheelSpeeds.rightMetersPerSecond);
+
+    // Calculate maximum/minimum possible accelerations from motor dynamics
+    // and max/min wheel speeds
+    double maxWheelAcceleration =
+        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+    double minWheelAcceleration =
+        m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+    // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+    // increased by half of the trackwidth T.  Inner wheel has radius decreased
+    // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
+    // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
+    // Inner wheel is similar.
+
+    // sgn(speed) term added to correctly account for which wheel is on
+    // outside of turn:
+    // If moving forward, max acceleration constraint corresponds to wheel on outside of turn
+    // If moving backward, max acceleration constraint corresponds to wheel on inside of turn
+
+    double maxChassisAcceleration =
+        maxWheelAcceleration
+            / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
+            * Math.signum(velocityMetersPerSecond) / 2);
+    double minChassisAcceleration =
+        minWheelAcceleration
+            / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
+            * Math.signum(velocityMetersPerSecond) / 2);
+
+    // Negate acceleration of wheel on inside of turn if center of turn is inside of wheelbase
+    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
+      if (velocityMetersPerSecond > 0) {
+        minChassisAcceleration = -minChassisAcceleration;
+      } else {
+        maxChassisAcceleration = -maxChassisAcceleration;
+      }
+    }
+
+    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
new file mode 100644
index 0000000..dfa2583
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+
+/**
+ * A class that enforces constraints on the mecanum drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for all 4 wheels of the drivetrain stay below a certain
+ * limit.
+ */
+public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final MecanumDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a mecanum drive dynamics constraint.
+   *
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
+                                               double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
+        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
new file mode 100644
index 0000000..8ca63f4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for all 4 wheels of the drivetrain stay below a certain
+ * limit.
+ */
+public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final SwerveDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a swerve drive dynamics constraint.
+   *
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
+                                               double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
+        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
+    SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
new file mode 100644
index 0000000..b338c3f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * An interface for defining user-defined velocity and acceleration constraints
+ * while generating trajectories.
+ */
+public interface TrajectoryConstraint {
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                       double velocityMetersPerSecond);
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
+                                                double velocityMetersPerSecond);
+
+  /**
+   * Represents a minimum and maximum acceleration.
+   */
+  @SuppressWarnings("MemberName")
+  class MinMax {
+    public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
+    public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
+
+    /**
+     * Constructs a MinMax.
+     *
+     * @param minAccelerationMetersPerSecondSq The minimum acceleration.
+     * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
+     */
+    public MinMax(double minAccelerationMetersPerSecondSq,
+                  double maxAccelerationMetersPerSecondSq) {
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    /**
+     * Constructs a MinMax with default values.
+     */
+    public MinMax() {
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
new file mode 100644
index 0000000..e4d5b69
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -0,0 +1,798 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.Objects;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Represents colors.
+ *
+ * <p>Limited to 12 bits of precision.
+ */
+@SuppressWarnings("MemberName")
+public class Color {
+  /*
+   * FIRST Colors
+   */
+
+  /**
+   * #1560BD.
+   */
+  public static final Color kDenim = new Color(0.0823529412, 0.376470589, 0.7411764706);
+
+  /**
+   * #0066B3.
+   */
+  public static final Color kFirstBlue = new Color(0.0, 0.4, 0.7019607844);
+
+  /**
+   * #ED1C24.
+   */
+  public static final Color kFirstRed = new Color(0.9294117648, 0.1098039216, 0.1411764706);
+
+  /*
+   * Standard Colors
+   */
+
+  /**
+   * #F0F8FF.
+   */
+  public static final Color kAliceBlue = new Color(0.9411765f, 0.972549f, 1.0f);
+
+  /**
+   * #FAEBD7.
+   */
+  public static final Color kAntiqueWhite = new Color(0.98039216f, 0.92156863f, 0.84313726f);
+
+  /**
+   * #00FFFF.
+   */
+  public static final Color kAqua = new Color(0.0f, 1.0f, 1.0f);
+
+  /**
+   * #7FFFD4.
+    */
+  public static final Color kAquamarine = new Color(0.49803922f, 1.0f, 0.83137256f);
+
+  /**
+   * #F0FFFF.
+   */
+  public static final Color kAzure = new Color(0.9411765f, 1.0f, 1.0f);
+
+  /**
+   * #F5F5DC.
+   */
+  public static final Color kBeige = new Color(0.9607843f, 0.9607843f, 0.8627451f);
+
+  /**
+   * #FFE4C4.
+   */
+  public static final Color kBisque = new Color(1.0f, 0.89411765f, 0.76862746f);
+
+  /**
+   * #000000.
+   */
+  public static final Color kBlack = new Color(0.0f, 0.0f, 0.0f);
+
+  /**
+   * #FFEBCD.
+   */
+  public static final Color kBlanchedAlmond = new Color(1.0f, 0.92156863f, 0.8039216f);
+
+  /**
+   * #0000FF.
+   */
+  public static final Color kBlue = new Color(0.0f, 0.0f, 1.0f);
+
+  /**
+   * #8A2BE2.
+   */
+  public static final Color kBlueViolet = new Color(0.5411765f, 0.16862746f, 0.8862745f);
+
+  /**
+   * #A52A2A.
+   */
+  public static final Color kBrown = new Color(0.64705884f, 0.16470589f, 0.16470589f);
+
+  /**
+   * #DEB887.
+   */
+  public static final Color kBurlywood = new Color(0.87058824f, 0.72156864f, 0.5294118f);
+
+  /**
+   * #5F9EA0.
+   */
+  public static final Color kCadetBlue = new Color(0.37254903f, 0.61960787f, 0.627451f);
+
+  /**
+   * #7FFF00.
+   */
+  public static final Color kChartreuse = new Color(0.49803922f, 1.0f, 0.0f);
+
+  /**
+   * #D2691E.
+   */
+  public static final Color kChocolate = new Color(0.8235294f, 0.4117647f, 0.11764706f);
+
+  /**
+   * #FF7F50.
+   */
+  public static final Color kCoral = new Color(1.0f, 0.49803922f, 0.3137255f);
+
+  /**
+   * #6495ED.
+   */
+  public static final Color kCornflowerBlue = new Color(0.39215687f, 0.58431375f, 0.92941177f);
+
+  /**
+   * #FFF8DC.
+   */
+  public static final Color kCornsilk = new Color(1.0f, 0.972549f, 0.8627451f);
+
+  /**
+   * #DC143C.
+   */
+  public static final Color kCrimson = new Color(0.8627451f, 0.078431375f, 0.23529412f);
+
+  /**
+   * #00FFFF.
+   */
+  public static final Color kCyan = new Color(0.0f, 1.0f, 1.0f);
+
+  /**
+   * #00008B.
+   */
+  public static final Color kDarkBlue = new Color(0.0f, 0.0f, 0.54509807f);
+
+  /**
+   * #008B8B.
+   */
+  public static final Color kDarkCyan = new Color(0.0f, 0.54509807f, 0.54509807f);
+
+  /**
+   * #B8860B.
+   */
+  public static final Color kDarkGoldenrod = new Color(0.72156864f, 0.5254902f, 0.043137256f);
+
+  /**
+   * #A9A9A9.
+   */
+  public static final Color kDarkGray = new Color(0.6627451f, 0.6627451f, 0.6627451f);
+
+  /**
+   * #006400.
+   */
+  public static final Color kDarkGreen = new Color(0.0f, 0.39215687f, 0.0f);
+
+  /**
+   * #BDB76B.
+   */
+  public static final Color kDarkKhaki = new Color(0.7411765f, 0.7176471f, 0.41960785f);
+
+  /**
+   * #8B008B.
+   */
+  public static final Color kDarkMagenta = new Color(0.54509807f, 0.0f, 0.54509807f);
+
+  /**
+   * #556B2F.
+   */
+  public static final Color kDarkOliveGreen = new Color(0.33333334f, 0.41960785f, 0.18431373f);
+
+  /**
+   * #FF8C00.
+   */
+  public static final Color kDarkOrange = new Color(1.0f, 0.54901963f, 0.0f);
+
+  /**
+   * #9932CC.
+   */
+  public static final Color kDarkOrchid = new Color(0.6f, 0.19607843f, 0.8f);
+
+  /**
+   * #8B0000.
+   */
+  public static final Color kDarkRed = new Color(0.54509807f, 0.0f, 0.0f);
+
+  /**
+   * #E9967A.
+   */
+  public static final Color kDarkSalmon = new Color(0.9137255f, 0.5882353f, 0.47843137f);
+
+  /**
+   * #8FBC8F.
+   */
+  public static final Color kDarkSeaGreen = new Color(0.56078434f, 0.7372549f, 0.56078434f);
+
+  /**
+   * #483D8B.
+   */
+  public static final Color kDarkSlateBlue = new Color(0.28235295f, 0.23921569f, 0.54509807f);
+
+  /**
+   * #2F4F4F.
+   */
+  public static final Color kDarkSlateGray = new Color(0.18431373f, 0.30980393f, 0.30980393f);
+
+  /**
+   * #00CED1.
+   */
+  public static final Color kDarkTurquoise = new Color(0.0f, 0.80784315f, 0.81960785f);
+
+  /**
+   * #9400D3.
+   */
+  public static final Color kDarkViolet = new Color(0.5803922f, 0.0f, 0.827451f);
+
+  /**
+   * #FF1493.
+   */
+  public static final Color kDeepPink = new Color(1.0f, 0.078431375f, 0.5764706f);
+
+  /**
+   * #00BFFF.
+   */
+  public static final Color kDeepSkyBlue = new Color(0.0f, 0.7490196f, 1.0f);
+
+  /**
+   * #696969.
+   */
+  public static final Color kDimGray = new Color(0.4117647f, 0.4117647f, 0.4117647f);
+
+  /**
+   * #1E90FF.
+   */
+  public static final Color kDodgerBlue = new Color(0.11764706f, 0.5647059f, 1.0f);
+
+  /**
+   * #B22222.
+   */
+  public static final Color kFirebrick = new Color(0.69803923f, 0.13333334f, 0.13333334f);
+
+  /**
+   * #FFFAF0.
+   */
+  public static final Color kFloralWhite = new Color(1.0f, 0.98039216f, 0.9411765f);
+
+  /**
+   * #228B22.
+   */
+  public static final Color kForestGreen = new Color(0.13333334f, 0.54509807f, 0.13333334f);
+
+  /**
+   * #FF00FF.
+   */
+  public static final Color kFuchsia = new Color(1.0f, 0.0f, 1.0f);
+
+  /**
+   * #DCDCDC.
+   */
+  public static final Color kGainsboro = new Color(0.8627451f, 0.8627451f, 0.8627451f);
+
+  /**
+   * #F8F8FF.
+   */
+  public static final Color kGhostWhite = new Color(0.972549f, 0.972549f, 1.0f);
+
+  /**
+   * #FFD700.
+   */
+  public static final Color kGold = new Color(1.0f, 0.84313726f, 0.0f);
+
+  /**
+   * #DAA520.
+   */
+  public static final Color kGoldenrod = new Color(0.85490197f, 0.64705884f, 0.1254902f);
+
+  /**
+   * #808080.
+   */
+  public static final Color kGray = new Color(0.5019608f, 0.5019608f, 0.5019608f);
+
+  /**
+   * #008000.
+   */
+  public static final Color kGreen = new Color(0.0f, 0.5019608f, 0.0f);
+
+  /**
+   * #ADFF2F.
+   */
+  public static final Color kGreenYellow = new Color(0.6784314f, 1.0f, 0.18431373f);
+
+  /**
+   * #F0FFF0.
+   */
+  public static final Color kHoneydew = new Color(0.9411765f, 1.0f, 0.9411765f);
+
+  /**
+   * #FF69B4.
+   */
+  public static final Color kHotPink = new Color(1.0f, 0.4117647f, 0.7058824f);
+
+  /**
+   * #CD5C5C.
+   */
+  public static final Color kIndianRed = new Color(0.8039216f, 0.36078432f, 0.36078432f);
+
+  /**
+   * #4B0082.
+   */
+  public static final Color kIndigo = new Color(0.29411766f, 0.0f, 0.50980395f);
+
+  /**
+   * #FFFFF0.
+   */
+  public static final Color kIvory = new Color(1.0f, 1.0f, 0.9411765f);
+
+  /**
+   * #F0E68C.
+   */
+  public static final Color kKhaki = new Color(0.9411765f, 0.9019608f, 0.54901963f);
+
+  /**
+   * #E6E6FA.
+   */
+  public static final Color kLavender = new Color(0.9019608f, 0.9019608f, 0.98039216f);
+
+  /**
+   * #FFF0F5.
+   */
+  public static final Color kLavenderBlush = new Color(1.0f, 0.9411765f, 0.9607843f);
+
+  /**
+   * #7CFC00.
+   */
+  public static final Color kLawnGreen = new Color(0.4862745f, 0.9882353f, 0.0f);
+
+  /**
+   * #FFFACD.
+   */
+  public static final Color kLemonChiffon = new Color(1.0f, 0.98039216f, 0.8039216f);
+
+  /**
+   * #ADD8E6.
+   */
+  public static final Color kLightBlue = new Color(0.6784314f, 0.84705883f, 0.9019608f);
+
+  /**
+   * #F08080.
+   */
+  public static final Color kLightCoral = new Color(0.9411765f, 0.5019608f, 0.5019608f);
+
+  /**
+   * #E0FFFF.
+   */
+  public static final Color kLightCyan = new Color(0.8784314f, 1.0f, 1.0f);
+
+  /**
+   * #FAFAD2.
+   */
+  public static final Color kLightGoldenrodYellow = new Color(0.98039216f, 0.98039216f, 0.8235294f);
+
+  /**
+   * #D3D3D3.
+   */
+  public static final Color kLightGray = new Color(0.827451f, 0.827451f, 0.827451f);
+
+  /**
+   * #90EE90.
+   */
+  public static final Color kLightGreen = new Color(0.5647059f, 0.93333334f, 0.5647059f);
+
+  /**
+   * #FFB6C1.
+   */
+  public static final Color kLightPink = new Color(1.0f, 0.7137255f, 0.75686276f);
+
+  /**
+   * #FFA07A.
+   */
+  public static final Color kLightSalmon = new Color(1.0f, 0.627451f, 0.47843137f);
+
+  /**
+   * #20B2AA.
+   */
+  public static final Color kLightSeagGeen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
+
+  /**
+   * #87CEFA.
+   */
+  public static final Color kLightSkyBlue = new Color(0.5294118f, 0.80784315f, 0.98039216f);
+
+  /**
+   * #778899.
+   */
+  public static final Color kLightSlateGray = new Color(0.46666667f, 0.53333336f, 0.6f);
+
+  /**
+   * #B0C4DE.
+   */
+  public static final Color kLightSteellue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
+
+  /**
+   * #FFFFE0.
+   */
+  public static final Color kLightYellow = new Color(1.0f, 1.0f, 0.8784314f);
+
+  /**
+   * #00FF00.
+   */
+  public static final Color kLime = new Color(0.0f, 1.0f, 0.0f);
+
+  /**
+   * #32CD32.
+   */
+  public static final Color kLimeGreen = new Color(0.19607843f, 0.8039216f, 0.19607843f);
+
+  /**
+   * #FAF0E6.
+   */
+  public static final Color kLinen = new Color(0.98039216f, 0.9411765f, 0.9019608f);
+
+  /**
+   * #FF00FF.
+   */
+  public static final Color kMagenta = new Color(1.0f, 0.0f, 1.0f);
+
+  /**
+   * #800000.
+   */
+  public static final Color kMaroon = new Color(0.5019608f, 0.0f, 0.0f);
+
+  /**
+   * #66CDAA.
+   */
+  public static final Color kMediumAquamarine = new Color(0.4f, 0.8039216f, 0.6666667f);
+
+  /**
+   * #0000CD.
+   */
+  public static final Color kMediumBlue = new Color(0.0f, 0.0f, 0.8039216f);
+
+  /**
+   * #BA55D3.
+   */
+  public static final Color kMediumOrchid = new Color(0.7294118f, 0.33333334f, 0.827451f);
+
+  /**
+   * #9370DB.
+   */
+  public static final Color kMediumPurple = new Color(0.5764706f, 0.4392157f, 0.85882354f);
+
+  /**
+   * #3CB371.
+   */
+  public static final Color kMediumSeaGreen = new Color(0.23529412f, 0.7019608f, 0.44313726f);
+
+  /**
+   * #7B68EE.
+   */
+  public static final Color kMediumSlateBlue = new Color(0.48235294f, 0.40784314f, 0.93333334f);
+
+  /**
+   * #00FA9A.
+   */
+  public static final Color kMediumSpringGreen = new Color(0.0f, 0.98039216f, 0.6039216f);
+
+  /**
+   * #48D1CC.
+   */
+  public static final Color kMediumTurquoise = new Color(0.28235295f, 0.81960785f, 0.8f);
+
+  /**
+   * #C71585.
+   */
+  public static final Color kMediumVioletRed = new Color(0.78039217f, 0.08235294f, 0.52156866f);
+
+  /**
+   * #191970.
+   */
+  public static final Color kMidnightBlue = new Color(0.09803922f, 0.09803922f, 0.4392157f);
+
+  /**
+   * #F5FFFA.
+   */
+  public static final Color kMintcream = new Color(0.9607843f, 1.0f, 0.98039216f);
+
+  /**
+   * #FFE4E1.
+   */
+  public static final Color kMistyRose = new Color(1.0f, 0.89411765f, 0.88235295f);
+
+  /**
+   * #FFE4B5.
+   */
+  public static final Color kMoccasin = new Color(1.0f, 0.89411765f, 0.70980394f);
+
+  /**
+   * #FFDEAD.
+   */
+  public static final Color kNavajoWhite = new Color(1.0f, 0.87058824f, 0.6784314f);
+
+  /**
+   * #000080.
+   */
+  public static final Color kNavy = new Color(0.0f, 0.0f, 0.5019608f);
+
+  /**
+   * #FDF5E6.
+   */
+  public static final Color kOldLace = new Color(0.99215686f, 0.9607843f, 0.9019608f);
+
+  /**
+   * #808000.
+   */
+  public static final Color kOlive = new Color(0.5019608f, 0.5019608f, 0.0f);
+
+  /**
+   * #6B8E23.
+   */
+  public static final Color kOliveDrab = new Color(0.41960785f, 0.5568628f, 0.13725491f);
+
+  /**
+   * #FFA500.
+   */
+  public static final Color kOrange = new Color(1.0f, 0.64705884f, 0.0f);
+
+  /**
+   * #FF4500.
+   */
+  public static final Color kOrangeRed = new Color(1.0f, 0.27058825f, 0.0f);
+
+  /**
+   * #DA70D6.
+   */
+  public static final Color kOrchid = new Color(0.85490197f, 0.4392157f, 0.8392157f);
+
+  /**
+   * #EEE8AA.
+   */
+  public static final Color kPaleGoldenrod = new Color(0.93333334f, 0.9098039f, 0.6666667f);
+
+  /**
+   * #98FB98.
+   */
+  public static final Color kPaleGreen = new Color(0.59607846f, 0.9843137f, 0.59607846f);
+
+  /**
+   * #AFEEEE.
+   */
+  public static final Color kPaleTurquoise = new Color(0.6862745f, 0.93333334f, 0.93333334f);
+
+  /**
+   * #DB7093.
+   */
+  public static final Color kPaleVioletRed = new Color(0.85882354f, 0.4392157f, 0.5764706f);
+
+  /**
+   * #FFEFD5.
+   */
+  public static final Color kPapayaWhip = new Color(1.0f, 0.9372549f, 0.8352941f);
+
+  /**
+   * #FFDAB9.
+   */
+  public static final Color kPeachPuff = new Color(1.0f, 0.85490197f, 0.7254902f);
+
+  /**
+   * #CD853F.
+   */
+  public static final Color kPeru = new Color(0.8039216f, 0.52156866f, 0.24705882f);
+
+  /**
+   * #FFC0CB.
+   */
+  public static final Color kPink = new Color(1.0f, 0.7529412f, 0.79607844f);
+
+  /**
+   * #DDA0DD.
+   */
+  public static final Color kPlum = new Color(0.8666667f, 0.627451f, 0.8666667f);
+
+  /**
+   * #B0E0E6.
+   */
+  public static final Color kPowderBlue = new Color(0.6901961f, 0.8784314f, 0.9019608f);
+
+  /**
+   * #800080.
+   */
+  public static final Color kPurple = new Color(0.5019608f, 0.0f, 0.5019608f);
+
+  /**
+   * #FF0000.
+   */
+  public static final Color kRed = new Color(1.0f, 0.0f, 0.0f);
+
+  /**
+   * #BC8F8F.
+   */
+  public static final Color kRosyBrown = new Color(0.7372549f, 0.56078434f, 0.56078434f);
+
+  /**
+   * #4169E1.
+   */
+  public static final Color kRoyalBlue = new Color(0.25490198f, 0.4117647f, 0.88235295f);
+
+  /**
+   * #8B4513.
+   */
+  public static final Color kSaddleBrown = new Color(0.54509807f, 0.27058825f, 0.07450981f);
+
+  /**
+   * #FA8072.
+   */
+  public static final Color kSalmon = new Color(0.98039216f, 0.5019608f, 0.44705883f);
+
+  /**
+   * #F4A460.
+   */
+  public static final Color kSandyBrown = new Color(0.95686275f, 0.6431373f, 0.3764706f);
+
+  /**
+   * #2E8B57.
+   */
+  public static final Color kSeaGreen = new Color(0.18039216f, 0.54509807f, 0.34117648f);
+
+  /**
+   * #FFF5EE.
+   */
+  public static final Color kSeashell = new Color(1.0f, 0.9607843f, 0.93333334f);
+
+  /**
+   * #A0522D.
+   */
+  public static final Color kSienna = new Color(0.627451f, 0.32156864f, 0.1764706f);
+
+  /**
+   * #C0C0C0.
+   */
+  public static final Color kSilver = new Color(0.7529412f, 0.7529412f, 0.7529412f);
+
+  /**
+   * #87CEEB.
+   */
+  public static final Color kSkyBlue = new Color(0.5294118f, 0.80784315f, 0.92156863f);
+
+  /**
+   * #6A5ACD.
+   */
+  public static final Color kSlateBlue = new Color(0.41568628f, 0.3529412f, 0.8039216f);
+
+  /**
+   * #708090.
+   */
+  public static final Color kSlateGray = new Color(0.4392157f, 0.5019608f, 0.5647059f);
+
+  /**
+   * #FFFAFA.
+   */
+  public static final Color kSnow = new Color(1.0f, 0.98039216f, 0.98039216f);
+
+  /**
+   * #00FF7F.
+   */
+  public static final Color kSpringGreen = new Color(0.0f, 1.0f, 0.49803922f);
+
+  /**
+   * #4682B4.
+   */
+  public static final Color kSteelBlue = new Color(0.27450982f, 0.50980395f, 0.7058824f);
+
+  /**
+   * #D2B48C.
+   */
+  public static final Color kTan = new Color(0.8235294f, 0.7058824f, 0.54901963f);
+
+  /**
+   * #008080.
+   */
+  public static final Color kTeal = new Color(0.0f, 0.5019608f, 0.5019608f);
+
+  /**
+   * #D8BFD8.
+   */
+  public static final Color kThistle = new Color(0.84705883f, 0.7490196f, 0.84705883f);
+
+  /**
+   * #FF6347.
+   */
+  public static final Color kTomato = new Color(1.0f, 0.3882353f, 0.2784314f);
+
+  /**
+   * #40E0D0.
+   */
+  public static final Color kTurquoise = new Color(0.2509804f, 0.8784314f, 0.8156863f);
+
+  /**
+   * #EE82EE.
+   */
+  public static final Color kViolet = new Color(0.93333334f, 0.50980395f, 0.93333334f);
+
+  /**
+   * #F5DEB3.
+   */
+  public static final Color kWheat = new Color(0.9607843f, 0.87058824f, 0.7019608f);
+
+  /**
+   * #FFFFFF.
+   */
+  public static final Color kWhite = new Color(1.0f, 1.0f, 1.0f);
+
+  /**
+   * #F5F5F5.
+   */
+  public static final Color kWhiteSmoke = new Color(0.9607843f, 0.9607843f, 0.9607843f);
+
+  /**
+   * #FFFF00.
+   */
+  public static final Color kYellow = new Color(1.0f, 1.0f, 0.0f);
+
+  /**
+   * #9ACD32.
+   */
+  public static final Color kYellowGreen = new Color(0.6039216f, 0.8039216f, 0.19607843f);
+
+  public final double red;
+  public final double green;
+  public final double blue;
+
+  private static final double kPrecision = Math.pow(2, -12);
+
+  /**
+   * Constructs a Color.
+   *
+   * @param red   Red value (0-1)
+   * @param green Green value (0-1)
+   * @param blue  Blue value (0-1)
+   */
+  Color(double red, double green, double blue) {
+    this.red = roundAndClamp(red);
+    this.green = roundAndClamp(green);
+    this.blue = roundAndClamp(blue);
+  }
+
+  /**
+   * Constructs a Color from a Color8Bit.
+   *
+   * @param color The color
+   */
+  public Color(Color8Bit color) {
+    this(color.red / 255.0,
+        color.green / 255.0,
+        color.blue / 255.0);
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null || getClass() != other.getClass()) {
+      return false;
+    }
+
+    Color color = (Color) other;
+    return Double.compare(color.red, red) == 0
+        && Double.compare(color.green, green) == 0
+        && Double.compare(color.blue, blue) == 0;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(red, green, blue);
+  }
+
+  private static double roundAndClamp(double value) {
+    final var rounded = Math.round(value / kPrecision) * kPrecision;
+    return MathUtil.clamp(rounded, 0.0, 1.0);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
new file mode 100644
index 0000000..50d42fc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.Objects;
+
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Represents colors with 8 bits of precision.
+ */
+@SuppressWarnings("MemberName")
+public class Color8Bit {
+  public final int red;
+  public final int green;
+  public final int blue;
+
+  /**
+   * Constructs a Color8Bit.
+   *
+   * @param red   Red value (0-255)
+   * @param green Green value (0-255)
+   * @param blue  Blue value (0-255)
+   */
+  public Color8Bit(int red, int green, int blue) {
+    this.red = MathUtil.clamp(red, 0, 255);
+    this.green = MathUtil.clamp(green, 0, 255);
+    this.blue = MathUtil.clamp(blue, 0, 255);
+  }
+
+  /**
+   * Constructs a Color8Bit from a Color.
+   *
+   * @param color The color
+   */
+  public Color8Bit(Color color) {
+    this((int) (color.red * 255),
+        (int) (color.green * 255),
+        (int) (color.blue * 255));
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null || getClass() != other.getClass()) {
+      return false;
+    }
+
+    Color8Bit color8Bit = (Color8Bit) other;
+    return red == color8Bit.red
+        && green == color8Bit.green
+        && blue == color8Bit.blue;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(red, green, blue);
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
new file mode 100644
index 0000000..8bc86b2
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 static java.util.Objects.requireNonNull;
+
+/**
+ * Utility class for common WPILib error messages.
+ */
+public final class ErrorMessages {
+  /**
+   * Utility class, so constructor is private.
+   */
+  private ErrorMessages() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Requires that a parameter of a method not be null; prints an error message with
+   * helpful debugging instructions if the parameter is null.
+   *
+   * @param obj The parameter that must not be null.
+   * @param paramName The name of the parameter.
+   * @param methodName The name of the method.
+   */
+  public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
+    return requireNonNull(obj,
+        "Parameter " + paramName + " in method " + methodName + " was null when it"
+            + " should not have been!  Check the stacktrace to find the responsible line of code - "
+            + "usually, it is the first line of user-written code indicated in the stacktrace.  "
+            + "Make sure all objects passed to the method in question were properly initialized -"
+            + " note that this may not be obvious if it is being called under "
+            + "dynamically-changing conditions!  Please do not seek additional technical assistance"
+            + " without doing this first!");
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java
new file mode 100644
index 0000000..1df7d33
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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;
+
+/**
+ * Utility class that converts between commonly used units in FRC.
+ */
+public final class Units {
+  private static final double kInchesPerFoot = 12.0;
+  private static final double kMetersPerInch = 0.0254;
+  private static final double kSecondsPerMinute = 60;
+
+  /**
+   * Utility class, so constructor is private.
+   */
+  private Units() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Converts given meters to feet.
+   *
+   * @param meters The meters to convert to feet.
+   * @return Feet converted from meters.
+   */
+  public static double metersToFeet(double meters) {
+    return metersToInches(meters) / kInchesPerFoot;
+  }
+
+  /**
+   * Converts given feet to meters.
+   *
+   * @param feet The feet to convert to meters.
+   * @return Meters converted from feet.
+   */
+  public static double feetToMeters(double feet) {
+    return inchesToMeters(feet * kInchesPerFoot);
+  }
+
+  /**
+   * Converts given meters to inches.
+   *
+   * @param meters The meters to convert to inches.
+   * @return Inches converted from meters.
+   */
+  public static double metersToInches(double meters) {
+    return meters / kMetersPerInch;
+  }
+
+  /**
+   * Converts given inches to meters.
+   *
+   * @param inches The inches to convert to meters.
+   * @return Meters converted from inches.
+   */
+  public static double inchesToMeters(double inches) {
+    return inches * kMetersPerInch;
+  }
+
+  /**
+   * Converts given degrees to radians.
+   *
+   * @param degrees The degrees to convert to radians.
+   * @return Radians converted from degrees.
+   */
+  public static double degreesToRadians(double degrees) {
+    return Math.toRadians(degrees);
+  }
+
+  /**
+   * Converts given radians to degrees.
+   *
+   * @param radians The radians to convert to degrees.
+   * @return Degrees converted from radians.
+   */
+  public static double radiansToDegrees(double radians) {
+    return Math.toDegrees(radians);
+  }
+
+  /**
+   * Converts rotations per minute to radians per second.
+   *
+   * @param rpm The rotations per minute to convert to radians per second.
+   * @return Radians per second converted from rotations per minute.
+   */
+  public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
+    return rpm * Math.PI / (kSecondsPerMinute / 2);
+  }
+
+  /**
+   * Converts radians per second to rotations per minute.
+   *
+   * @param radiansPerSecond The radians per second to convert to from rotations per minute.
+   * @return Rotations per minute converted from radians per second.
+   */
+  public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
+    return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
+  }
+}
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..328686a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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) {
+      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) {
+      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&lt;MyFindTotePipeline&gt; {
+ *
+ *      // 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/AddressableLEDBufferTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
new file mode 100644
index 0000000..13f1651
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.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.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class AddressableLEDBufferTest {
+  @ParameterizedTest
+  @MethodSource("hsvToRgbProvider")
+  @SuppressWarnings("ParameterName")
+  void hsvConvertTest(int h, int s, int v, int r, int g, int b) {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setHSV(0, h, s, v);
+    assertAll(
+        () -> assertEquals((byte) r, buffer.m_buffer[2], "R value didn't match"),
+        () -> assertEquals((byte) g, buffer.m_buffer[1], "G value didn't match"),
+        () -> assertEquals((byte) b, buffer.m_buffer[0], "B value didn't match")
+    );
+  }
+
+  static Stream<Arguments> hsvToRgbProvider() {
+    return Stream.of(
+        arguments(0, 0, 0, 0, 0, 0), // Black
+        arguments(0, 0, 255, 255, 255, 255), // White
+        arguments(0, 255, 255, 255, 0, 0), // Red
+        arguments(60, 255, 255, 0, 255, 0), // Lime
+        arguments(120, 255, 255, 0, 0, 255), // Blue
+        arguments(30, 255, 255, 254, 255, 0), // Yellow (ish)
+        arguments(90, 255, 255, 0, 254, 255), // Cyan (ish)
+        arguments(150, 255, 255, 255, 0, 254), // Magenta (ish)
+        arguments(0, 0, 191, 191, 191, 191), // Silver
+        arguments(0, 0, 128, 128, 128, 128), // Gray
+        arguments(0, 255, 128, 128, 0, 0), // Maroon
+        arguments(30, 255, 128, 127, 128, 0), // Olive (ish)
+        arguments(60, 255, 128, 0, 128, 0), // Green
+        arguments(150, 255, 128, 128, 0, 127), // Purple (ish)
+        arguments(90, 255, 128, 0, 127, 128), // Teal (ish)
+        arguments(120, 255, 128, 0, 0, 128) // Navy
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
new file mode 100644
index 0000000..3953c4c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 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.Random;
+import java.util.function.DoubleFunction;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.Test;
+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.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class LinearFilterTest {
+  private static final double kFilterStep = 0.005;
+  private static final double kFilterTime = 2.0;
+  private static final double kSinglePoleIIRTimeConstant = 0.015915;
+  private static final double kHighPassTimeConstant = 0.006631;
+  private static final int kMovAvgTaps = 6;
+
+  private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+  private static final double kHighPassExpectedOutput = 10.074717;
+  private static final double kMovAvgExpectedOutput = -10.191644;
+
+  @SuppressWarnings("ParameterName")
+  private static double getData(double t) {
+    return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
+  }
+
+  @SuppressWarnings("ParameterName")
+  private static double getPulseData(double t) {
+    if (Math.abs(t - 1.0) < 0.001) {
+      return 1.0;
+    } else {
+      return 0.0;
+    }
+  }
+
+  @Test
+  void illegalTapNumberTest() {
+    assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
+  }
+
+  /**
+   * Test if the filter reduces the noise produced by a signal generator.
+   */
+  @ParameterizedTest
+  @MethodSource("noiseFilterProvider")
+  void noiseReduceTest(final LinearFilter filter) {
+    double noiseGenError = 0.0;
+    double filterError = 0.0;
+
+    final Random gen = new Random();
+    final double kStdDev = 10.0;
+
+    for (double t = 0; t < kFilterTime; t += kFilterStep) {
+      final double theory = getData(t);
+      final double noise = gen.nextGaussian() * kStdDev;
+      filterError += Math.abs(filter.calculate(theory + noise) - theory);
+      noiseGenError += Math.abs(noise - theory);
+    }
+
+    assertTrue(noiseGenError > filterError,
+        "Filter should have reduced noise accumulation from " + noiseGenError
+            + " but failed. The filter error was " + filterError);
+  }
+
+  static Stream<LinearFilter> noiseFilterProvider() {
+    return Stream.of(
+        LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+        LinearFilter.movingAverage(kMovAvgTaps)
+    );
+  }
+
+  /**
+   * Test if the linear filters produce consistent output for a given data set.
+   */
+  @ParameterizedTest
+  @MethodSource("outputFilterProvider")
+  void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
+                  final double expectedOutput) {
+    double filterOutput = 0.0;
+    for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
+      filterOutput = filter.calculate(data.apply(t));
+    }
+
+    assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
+  }
+
+  static Stream<Arguments> outputFilterProvider() {
+    return Stream.of(
+        arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kSinglePoleIIRExpectedOutput),
+        arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kHighPassExpectedOutput),
+        arguments(LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kMovAvgExpectedOutput),
+        arguments(LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getPulseData,
+            0.0)
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
new file mode 100644
index 0000000..00b100d
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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;
+
+public class MedianFilterTest {
+  @Test
+  void medianFilterNotFullTestEven() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+
+    assertEquals(3.5, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterNotFullTestOdd() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+    filter.calculate(7);
+
+    assertEquals(4, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterFullTestEven() {
+    MedianFilter filter = new MedianFilter(6);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(4.5, filter.calculate(99));
+  }
+
+  @Test
+  void medianFilterFullTestOdd() {
+    MedianFilter filter = new MedianFilter(5);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(5, filter.calculate(99));
+  }
+}
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..d97b352
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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) {
+    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/PreferencesTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
new file mode 100644
index 0000000..7b3735f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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.IOException;
+import java.io.InputStream;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Arrays;
+import java.util.Set;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Nested;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.io.TempDir;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import edu.wpi.first.networktables.NetworkTable;
+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;
+import static org.junit.jupiter.api.Assertions.fail;
+
+class PreferencesTest {
+  private final Preferences m_prefs = Preferences.getInstance();
+  private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("Preferences");
+
+  private static final String kFilename = "networktables.ini";
+
+  @BeforeAll
+  static void setupAll() {
+    NetworkTableInstance.getDefault().stopServer();
+  }
+
+  @BeforeEach
+  void setup(@TempDir Path tempDir) {
+    m_table.getKeys().forEach(m_table::delete);
+
+    Path filepath = tempDir.resolve(kFilename);
+    try (InputStream is = getClass().getResource("PreferencesTestDefault.ini").openStream()) {
+      Files.copy(is, filepath);
+    } catch (IOException ex) {
+      fail(ex);
+    }
+
+    NetworkTableInstance.getDefault().startServer(filepath.toString());
+  }
+
+  @AfterEach
+  void cleanup() {
+    NetworkTableInstance.getDefault().stopServer();
+  }
+
+  @AfterAll
+  static void cleanupAll() {
+    NetworkTableInstance.getDefault().startServer();
+  }
+
+  @Test
+  void removeAllTest() {
+    m_prefs.removeAll();
+
+    Set<String> keys = m_table.getKeys();
+    keys.remove(".type");
+
+    assertTrue(keys.isEmpty(), "Preferences was not empty!  Preferences in table: "
+        + Arrays.toString(keys.toArray()));
+  }
+
+  @ParameterizedTest
+  @MethodSource("defaultKeyProvider")
+  void defaultKeysTest(String key) {
+    assertTrue(m_prefs.containsKey(key));
+  }
+
+  @ParameterizedTest
+  @MethodSource("defaultKeyProvider")
+  void defaultKeysAllTest(String key) {
+    assertTrue(m_prefs.getKeys().contains(key));
+  }
+
+  @Test
+  void defaultValueTest() {
+    assertAll(
+        () -> assertEquals(172L, m_prefs.getLong("checkedValueLong", 0)),
+        () -> assertEquals(0.2, m_prefs.getDouble("checkedValueDouble", 0), 1e-6),
+        () -> assertEquals("Hello. How are you?", m_prefs.getString("checkedValueString", "")),
+        () -> assertEquals(2, m_prefs.getInt("checkedValueInt", 0)),
+        () -> assertEquals(3.14, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
+        () -> assertFalse(m_prefs.getBoolean("checkedValueBoolean", true))
+    );
+  }
+
+  @Test
+  void backupTest() {
+    m_prefs.removeAll();
+
+    assertAll(
+        () -> assertEquals(0, m_prefs.getLong("checkedValueLong", 0)),
+        () -> assertEquals(0, m_prefs.getDouble("checkedValueDouble", 0), 1e-6),
+        () -> assertEquals("", m_prefs.getString("checkedValueString", "")),
+        () -> assertEquals(0, m_prefs.getInt("checkedValueInt", 0)),
+        () -> assertEquals(0, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
+        () -> assertTrue(m_prefs.getBoolean("checkedValueBoolean", true))
+    );
+  }
+
+  @Nested
+  class PutGetTests {
+    @Test
+    void intTest() {
+      final String key = "test";
+      final int value = 123;
+
+      m_prefs.putInt(key, value);
+
+      assertAll(
+          () -> assertEquals(value, m_prefs.getInt(key, -1)),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).intValue())
+      );
+    }
+
+    @Test
+    void longTest() {
+      final String key = "test";
+      final long value = 190L;
+
+      m_prefs.putLong(key, value);
+
+      assertAll(
+          () -> assertEquals(value, m_prefs.getLong(key, -1)),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).longValue())
+      );
+    }
+
+    @Test
+    void floatTest() {
+      final String key = "test";
+      final float value = 9.42f;
+
+      m_prefs.putFloat(key, value);
+
+      assertAll(
+          () -> assertEquals(value, m_prefs.getFloat(key, -1), 1e-6),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).floatValue(), 1e-6)
+      );
+    }
+
+    @Test
+    void doubleTest() {
+      final String key = "test";
+      final double value = 6.28;
+
+      m_prefs.putDouble(key, value);
+
+      assertAll(
+          () -> assertEquals(value, m_prefs.getDouble(key, -1), 1e-6),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).doubleValue(), 1e-6)
+      );
+    }
+
+    @Test
+    void stringTest() {
+      final String key = "test";
+      final String value = "value";
+
+      m_prefs.putString(key, value);
+
+      assertAll(
+          () -> assertEquals(value, m_prefs.getString(key, "")),
+          () -> assertEquals(value, m_table.getEntry(key).getString(""))
+      );
+    }
+
+    @Test
+    void booleanTest() {
+      final String key = "test";
+      final boolean value = true;
+
+      m_prefs.putBoolean(key, value);
+
+      assertAll(
+          () -> assertEquals(value, m_prefs.getBoolean(key, false)),
+          () -> assertEquals(value, m_table.getEntry(key).getBoolean(false))
+      );
+    }
+  }
+
+  static Stream<String> defaultKeyProvider() {
+    return Stream.of(
+        "checkedValueLong",
+        "checkedValueDouble",
+        "checkedValueString",
+        "checkedValueInt",
+        "checkedValueFloat",
+        "checkedValueBoolean"
+    );
+  }
+}
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..4bce18f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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<RobotController> {
+  RobotControllerTest() {
+    super(RobotController.class);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
new file mode 100644
index 0000000..e47b70b
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class SlewRateLimiterTest {
+  @Test
+  void slewRateLimitTest() {
+    SlewRateLimiter limiter = new SlewRateLimiter(1);
+    Timer.delay(1);
+    assertTrue(limiter.calculate(2) < 2);
+  }
+
+  @Test
+  void slewRateNoLimitTest() {
+    SlewRateLimiter limiter = new SlewRateLimiter(1);
+    Timer.delay(1);
+    assertEquals(limiter.calculate(0.5), 0.5);
+  }
+}
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..a66cd13
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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<T> {
+  private final Class<T> m_clazz;
+
+  protected UtilityClassTest(Class<T> 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.canAccess(null), "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..b956446
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 org.junit.jupiter.api.condition.DisabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@DisabledOnOs(OS.MAC)
+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");
+
+    watchdog.close();
+  }
+
+  @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");
+
+    watchdog.close();
+  }
+
+  @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");
+
+    watchdog.close();
+  }
+
+  @Test
+  void isExpiredTest() {
+    final Watchdog watchdog = new Watchdog(0.2, () -> {
+    });
+    assertFalse(watchdog.isExpired());
+    watchdog.enable();
+
+    assertFalse(watchdog.isExpired());
+    try {
+      Thread.sleep(300);
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    assertTrue(watchdog.isExpired());
+
+    watchdog.disable();
+    assertTrue(watchdog.isExpired());
+
+    watchdog.reset();
+    assertFalse(watchdog.isExpired());
+
+    watchdog.close();
+  }
+
+  @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");
+
+    watchdog.close();
+  }
+
+  @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");
+
+    watchdog1.close();
+    watchdog2.close();
+  }
+}
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/controller/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
new file mode 100644
index 0000000..873d03f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.controller;
+
+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.assertTrue;
+
+class PIDInputOutputTest {
+  private PIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new PIDController(0, 0, 0);
+  }
+
+  @Test
+  void continuousInputTest() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+
+    assertTrue(m_controller.calculate(-179, 179) < 0.0);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
new file mode 100644
index 0000000..cd2aba4
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2019 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.controller;
+
+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 static final double kSetpoint = 50.0;
+  private static final double kTolerance = 10.0;
+  private static final double kRange = 200;
+
+  @Test
+  void initialToleranceTest() {
+    var controller = new PIDController(0.05, 0.0, 0.0);
+    controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+    assertTrue(controller.atSetpoint());
+  }
+
+  @Test
+  void absoluteToleranceTest() {
+    var controller = new PIDController(0.05, 0.0, 0.0);
+    controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+    controller.setTolerance(kTolerance);
+    controller.setSetpoint(kSetpoint);
+
+    controller.calculate(0.0);
+
+    assertFalse(controller.atSetpoint(),
+        "Error was in tolerance when it should not have been. Error was " + controller
+            .getPositionError());
+
+    controller.calculate(kSetpoint + kTolerance / 2);
+
+    assertTrue(controller.atSetpoint(),
+        "Error was not in tolerance when it should have been. Error was " + controller
+            .getPositionError());
+
+    controller.calculate(kSetpoint + 10 * kTolerance);
+
+    assertFalse(controller.atSetpoint(),
+        "Error was in tolerance when it should not have been. Error was " + controller
+            .getPositionError());
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
new file mode 100644
index 0000000..6babcc2
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ProfiledPIDControllerTest {
+  @Test
+  void testStartFromNonZeroPosition() {
+    ProfiledPIDController controller = new ProfiledPIDController(1.0, 0.0, 0.0,
+        new TrapezoidProfile.Constraints(1.0, 1.0));
+
+    controller.reset(20);
+
+    assertEquals(0.0, controller.calculate(20), 0.05);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
new file mode 100644
index 0000000..2bef50d
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.controller;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class RamseteControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  private static double boundRadians(double value) {
+    while (value > Math.PI) {
+      value -= Math.PI * 2;
+    }
+    while (value <= -Math.PI) {
+      value += Math.PI * 2;
+    }
+    return value;
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    final var controller = new RamseteController(2.0, 0.7);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final double kDt = 0.02;
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+
+      var output = controller.calculate(robotPose, state);
+      robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt, 0,
+          output.omegaRadiansPerSecond * kDt));
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(),
+            kTolerance),
+        () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(),
+            kTolerance),
+        () -> assertEquals(0.0,
+            boundRadians(endPose.getRotation().getRadians()
+                - finalRobotPose.getRotation().getRadians()),
+            kAngularTolerance)
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java
new file mode 100644
index 0000000..14e9401
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java
@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.MockSpeedController;
+import edu.wpi.first.wpilibj.RobotDrive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Tests DifferentialDrive and MecanumDrive.
+ */
+public class DriveTest {
+  private final MockSpeedController m_rdFrontLeft = new MockSpeedController();
+  private final MockSpeedController m_rdRearLeft = new MockSpeedController();
+  private final MockSpeedController m_rdFrontRight = new MockSpeedController();
+  private final MockSpeedController m_rdRearRight = new MockSpeedController();
+  private final MockSpeedController m_frontLeft = new MockSpeedController();
+  private final MockSpeedController m_rearLeft = new MockSpeedController();
+  private final MockSpeedController m_frontRight = new MockSpeedController();
+  private final MockSpeedController m_rearRight = new MockSpeedController();
+  private final RobotDrive m_robotDrive =
+      new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight);
+  private final DifferentialDrive m_differentialDrive =
+      new DifferentialDrive(m_frontLeft, m_frontRight);
+  private final MecanumDrive m_mecanumDrive =
+      new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
+
+  private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9,
+                                                 -1.0};
+  private final double[] m_testGyroValues = {0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540,
+                                             -45, -90, -135, -180, -225, -270, -305, -360, -540};
+
+  @BeforeEach
+  void setUp() {
+    m_differentialDrive.setDeadband(0.0);
+    m_differentialDrive.setSafetyEnabled(false);
+    m_mecanumDrive.setDeadband(0.0);
+    m_mecanumDrive.setSafetyEnabled(false);
+    m_robotDrive.setSafetyEnabled(false);
+  }
+
+  @Test
+  public void testTankDriveSquared() {
+    for (double leftJoystick : m_testJoystickValues) {
+      for (double rightJoystick : m_testJoystickValues) {
+        m_robotDrive.tankDrive(leftJoystick, rightJoystick);
+        m_differentialDrive.tankDrive(leftJoystick, rightJoystick);
+        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+            "Left Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+            + rightJoystick);
+        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+            "Right Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+            + rightJoystick);
+      }
+    }
+  }
+
+  @Test
+  void testTankDrive() {
+    for (double leftJoystick : m_testJoystickValues) {
+      for (double rightJoystick : m_testJoystickValues) {
+        m_robotDrive.tankDrive(leftJoystick, rightJoystick, false);
+        m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false);
+        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+            "Left Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+            + rightJoystick);
+        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+            "Right Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
+            + rightJoystick);
+      }
+    }
+  }
+
+  @Test
+  void testArcadeDriveSquared() {
+    for (double moveJoystick : m_testJoystickValues) {
+      for (double rotateJoystick : m_testJoystickValues) {
+        m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick);
+        m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick);
+        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+            "Left Motor squared didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
+            + rotateJoystick);
+        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+            "Right Motor squared didn't match. Move Joystick: " + moveJoystick
+            + " Rotate Joystick: " + rotateJoystick);
+      }
+    }
+  }
+
+  @Test
+  void testArcadeDrive() {
+    for (double moveJoystick : m_testJoystickValues) {
+      for (double rotateJoystick : m_testJoystickValues) {
+        m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false);
+        m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false);
+        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+            "Left Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
+            + rotateJoystick);
+        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
+            "Right Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
+            + rotateJoystick);
+      }
+    }
+  }
+
+  @Test
+  void testMecanumPolar() {
+    for (double magnitudeJoystick : m_testJoystickValues) {
+      for (double directionJoystick : m_testGyroValues) {
+        for (double rotationJoystick : m_testJoystickValues) {
+          m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick);
+          m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick);
+          assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+              "Left Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+              + rotationJoystick);
+          assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01,
+              "Right Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+              + rotationJoystick);
+          assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01,
+              "Left Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+              + rotationJoystick);
+          assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01,
+              "Right Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
+              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
+              + rotationJoystick);
+        }
+      }
+    }
+  }
+
+  @Test
+  @SuppressWarnings("checkstyle:LocalVariableName")
+  void testMecanumCartesian() {
+    for (double x_Joystick : m_testJoystickValues) {
+      for (double y_Joystick : m_testJoystickValues) {
+        for (double rotationJoystick : m_testJoystickValues) {
+          for (double gyroValue : m_testGyroValues) {
+            m_robotDrive.mecanumDrive_Cartesian(x_Joystick, y_Joystick, rotationJoystick,
+                                                gyroValue);
+            m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue);
+            assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
+                "Left Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+            assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01,
+                "Right Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+            assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01,
+                "Left Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+            assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01,
+                "Right Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
+                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
+          }
+        }
+      }
+    }
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
new file mode 100644
index 0000000..066716e
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Pose2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testTransformBy() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+        Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRelativeTo() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0),
+            kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
+    assertNotEquals(one, two);
+  }
+
+  void testMinus() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    final var transform = last.minus(initial);
+
+    assertAll(
+        () -> assertEquals(transform.getTranslation().getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transform.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
new file mode 100644
index 0000000..6c4b1b3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Rotation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testRadiansToDegrees() {
+    var one = new Rotation2d(Math.PI / 3);
+    var two = new Rotation2d(Math.PI / 4);
+
+    assertAll(
+        () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
+        () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    var one = Rotation2d.fromDegrees(45.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertAll(
+        () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
+        () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByFromZero() {
+    var zero = new Rotation2d();
+    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
+        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByNonZero() {
+    var rot = Rotation2d.fromDegrees(90.0);
+    rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+  }
+
+  @Test
+  void testMinus() {
+    var one = Rotation2d.fromDegrees(70.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
+  }
+
+  @Test
+  void testEquality() {
+    var one = Rotation2d.fromDegrees(43.0);
+    var two = Rotation2d.fromDegrees(43.0);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = Rotation2d.fromDegrees(43.0);
+    var two = Rotation2d.fromDegrees(43.5);
+    assertNotEquals(one, two);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
new file mode 100644
index 0000000..b4e2947
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Translation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
+        () -> assertEquals(sum.getY(), 8.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
+        () -> assertEquals(difference.getY(), -2.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateBy() {
+    var another = new Translation2d(3.0, 0.0);
+    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
+        () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation2d(3.0, 5.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
+        () -> assertEquals(mult.getY(), 15.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation2d(3.0, 5.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(div.getX(), 1.5, kEpsilon),
+        () -> assertEquals(div.getY(), 2.5, kEpsilon)
+    );
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation2d(-4.5, 7);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
+        () -> assertEquals(inverted.getY(), -7, kEpsilon)
+    );
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.5);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.7);
+    assertNotEquals(one, two);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
new file mode 100644
index 0000000..903c436
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Twist2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testStraightLineTwist() {
+    var straight = new Twist2d(5.0, 0.0, 0.0);
+    var straightPose = new Pose2d().exp(straight);
+
+    assertAll(
+        () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testQuarterCirleTwist() {
+    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+    var quarterCirclePose = new Pose2d().exp(quarterCircle);
+
+    assertAll(
+        () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist2d(2.0, 2.0, 0.0);
+    var diagonalPose = new Pose2d().exp(diagonal);
+
+    assertAll(
+        () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1, 3);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1.2, 3);
+    assertNotEquals(one, two);
+  }
+
+  void testPose2dLog() {
+    final var start = new Pose2d();
+    final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
+
+    final var twist = start.log(end);
+
+    assertAll(
+        () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
+        () -> assertEquals(twist.dy, 0.0, kEpsilon),
+        () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
+    );
+  }
+}
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/kinematics/ChassisSpeedsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
new file mode 100644
index 0000000..729d7b8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ChassisSpeedsTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testFieldRelativeConstruction() {
+    final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+        1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
+    );
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
new file mode 100644
index 0000000..e484eab
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class DifferentialDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveKinematics m_kinematics
+      = new DifferentialDriveKinematics(0.381 * 2);
+
+  @Test
+  void testInverseKinematicsForZeros() {
+    var chassisSpeeds = new ChassisSpeeds();
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testForwardKinematicsForZeros() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds();
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testInverseKinematicsForStraightLine() {
+    var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testForwardKinematicsForStraightLine() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testInverseKinematicsForRotateInPlace() {
+    var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testForwardKinematicsForRotateInPlace() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
new file mode 100644
index 0000000..aabc455
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class DifferentialDriveOdometryTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
+      new Rotation2d());
+
+  @Test
+  void testOdometryWithEncoderDistances() {
+    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+    var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
+
+    assertAll(
+        () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
new file mode 100644
index 0000000..75b6f44
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class MecanumDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  @Test
+  void testStraightLineInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
+      */
+
+    assertAll(
+        () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testStraightLineForwardKinematicsKinematics() {
+
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
+      */
+
+    assertAll(
+        () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testStrafeInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
+      */
+
+    assertAll(
+        () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testStrafeForwardKinematicsKinematics() {
+
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
+      */
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
+      */
+
+    assertAll(
+        () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should be [[0][0][2pi]]
+      */
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testMixedTranslationRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
+      */
+
+    assertAll(
+        () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testMixedTranslationRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be [[2][3][1]]
+      */
+
+    assertAll(
+        () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
+      */
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
+    velocities should be [[12][-12][1]]
+      */
+
+    assertAll(
+        () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterTranslationRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
+      */
+
+    assertAll(
+        () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterRotationTranslationForwardKinematicsKinematics() {
+
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
+    velocities should be [[17][-10][1]]
+      */
+
+    assertAll(
+        () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testNormalize() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
+    wheelSpeeds.normalize(5.5);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
+    );
+  }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
new file mode 100644
index 0000000..4619bf0
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumDriveOdometryTest {
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+      new Rotation2d());
+
+  @Test
+  void testMultipleConsecutiveUpdates() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(secondPose.getTranslation().getX(), 0.0, 0.01),
+        () -> assertEquals(secondPose.getTranslation().getY(), 0.0, 0.01),
+        () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
+    );
+  }
+
+  @Test
+  void testTwoIterations() {
+    // 5 units/sec  in the x axis (forward)
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01),
+        () -> assertEquals(0, pose.getTranslation().getY(), 0.01),
+        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void test90degreeTurn() {
+    // This is a 90 degree turn about the point between front left and rear left wheels
+    // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01),
+        () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
+        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
+        3.536, 3.536);
+    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, pose.getTranslation().getX(), 0.1),
+        () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+    );
+  }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
new file mode 100644
index 0000000..f506436
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class SwerveDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  @Test
+  void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
+
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testStraightStrafeInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testStraightStrafeForwardKinematics() {
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testTurnInPlaceInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    /*
+    The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
+    the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
+    trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
+    our wheels must trace out 1 rotation (or 106.63 inches) per second.
+      */
+
+    assertAll(
+        () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
+        () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testTurnInPlaceForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
+    SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
+    SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
+    SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterCORRotationInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
+
+    /*
+    This one is a bit trickier. Because we are rotating about the front-left wheel,
+    it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
+    an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
+    radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
+    should be pointing straight forward, the back-left wheel should be pointing straight right,
+    and the back-right wheel should be at a -45 degree angle
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
+        () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
+        () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
+        () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
+        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testOffCenterCORRotationForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
+    SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
+    SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
+    SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    /*
+    We already know that our omega should be 2pi from the previous test. Next, we need to determine
+    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+    a full revolution about the center of revolution once every second. Therefore, the center of
+    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
+    1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+    */
+
+    assertAll(
+        () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
+        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
+                                 SwerveModuleState tolerance) {
+    assertAll(
+        () -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
+            tolerance.speedMetersPerSecond),
+        () -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
+            tolerance.angle.getDegrees())
+    );
+  }
+
+  /**
+   * Test the rotation of the robot about a non-central point with
+   * both linear and angular velocities.
+   */
+  @Test
+  void testOffCenterCORRotationAndTranslationInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
+
+    // By equation (13.14) from state-space guide, our wheels/angles will be as follows,
+    // (+-1 degree or speed):
+    SwerveModuleState[] expectedStates = new SwerveModuleState[]{
+        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
+        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
+        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
+        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
+    };
+    var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
+
+    for (int i = 0; i < expectedStates.length; i++) {
+      assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
+    }
+  }
+
+  @Test
+  void testOffCenterCORRotationAndTranslationForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
+    SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
+    SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
+    SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    /*
+    From equation (13.17), we know that chassis motion is th dot product of the
+    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+    (0,0) -- we don't want the motion of the center of rotation, we want it of
+    the center of the robot). These above SwerveModuleStates are known to be from
+    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+    calculated using Numpy's linalg.pinv function.
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testNormalize() {
+    SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
+    SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
+    SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
+    SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
+
+    SwerveModuleState[] arr = {fl, fr, bl, br};
+    SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
+    );
+  }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
new file mode 100644
index 0000000..85b832c
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveDriveOdometryTest {
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+      new Rotation2d());
+
+  @Test
+  void testTwoIterations() {
+    // 5 units/sec  in the x axis (forward)
+    final SwerveModuleState[] wheelSpeeds = {
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+    };
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(),
+        new SwerveModuleState(), new SwerveModuleState(),
+        new SwerveModuleState(), new SwerveModuleState());
+    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01),
+        () -> assertEquals(0, pose.getTranslation().getY(), 0.01),
+        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void test90degreeTurn() {
+    // This is a 90 degree turn about the point between front left and rear left wheels
+    //        Module 0: speed 18.84955592153876 angle 90.0
+    //        Module 1: speed 42.14888838624436 angle 26.565051177077986
+    //        Module 2: speed 18.84955592153876 angle -90.0
+    //        Module 3: speed 42.14888838624436 angle -26.565051177077986
+
+    final SwerveModuleState[] wheelSpeeds = {
+        new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
+        new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
+        new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
+        new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+    };
+    final var zero = new SwerveModuleState();
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
+    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01),
+        () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
+        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var state = new SwerveModuleState();
+    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
+    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
+    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(1.0, pose.getTranslation().getX(), 0.1),
+        () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+    );
+  }
+
+}
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..dd33945
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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<LiveWindow> {
+  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..89ca3ec
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A mock sendable that marks itself as an actuator.
+ */
+public class MockActuatorSendable implements Sendable {
+  public MockActuatorSendable(String name) {
+    SendableRegistry.add(this, 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/ShuffleboardTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
new file mode 100644
index 0000000..1469170
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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<Shuffleboard> {
+  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/shuffleboard/SuppliedValueWidgetTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
new file mode 100644
index 0000000..6e0da8e
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.concurrent.atomic.AtomicInteger;
+
+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.assertArrayEquals;
+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 SuppliedValueWidgetTest {
+  private NetworkTableInstance m_ntInstance;
+  private ShuffleboardInstance m_instance;
+
+  @BeforeEach
+  void setup() {
+    m_ntInstance = NetworkTableInstance.create();
+    m_instance = new ShuffleboardInstance(m_ntInstance);
+  }
+
+  @Test
+  void testAddString() {
+    AtomicInteger count = new AtomicInteger(0);
+    m_instance.getTab("Tab")
+        .addString("Title", () -> Integer.toString(count.incrementAndGet()));
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertEquals("1", entry.getString(null));
+
+    m_instance.update();
+    assertEquals("2", entry.getString(null));
+  }
+
+  @Test
+  void testAddDouble() {
+    AtomicInteger num = new AtomicInteger(0);
+    m_instance.getTab("Tab")
+        .addNumber("Title", num::incrementAndGet);
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertEquals(1, entry.getDouble(0));
+
+    m_instance.update();
+    assertEquals(2, entry.getDouble(0));
+  }
+
+  @Test
+  void testAddBoolean() {
+    boolean[] bool = {false};
+    m_instance.getTab("Tab")
+        .addBoolean("Title", () -> bool[0] = !bool[0]);
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertTrue(entry.getBoolean(false));
+
+    m_instance.update();
+    assertFalse(entry.getBoolean(true));
+  }
+
+  @Test
+  void testAddStringArray() {
+    String[] arr = {"foo", "bar"};
+    m_instance.getTab("Tab")
+        .addStringArray("Title", () -> arr);
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertArrayEquals(arr, entry.getStringArray(new String[0]));
+  }
+
+  @Test
+  void testAddDoubleArray() {
+    double[] arr = {0, 1};
+    m_instance.getTab("Tab")
+        .addDoubleArray("Title", () -> arr);
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertArrayEquals(arr, entry.getDoubleArray(new double[0]));
+  }
+
+  @Test
+  void testAddBooleanArray() {
+    boolean[] arr = {true, false};
+    m_instance.getTab("Tab")
+        .addBooleanArray("Title", () -> arr);
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertArrayEquals(arr, entry.getBooleanArray(new boolean[0]));
+  }
+
+  @Test
+  void testAddRawBytes() {
+    byte[] arr = {0, 1, 2, 3};
+    m_instance.getTab("Tab")
+        .addRaw("Title", () -> arr);
+    NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
+
+    m_instance.update();
+    assertArrayEquals(arr, entry.getRaw(new byte[0]));
+  }
+
+}
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..91210bc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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);
+
+    }
+
+    input.close();
+
+  }
+}
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..7771425
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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);
+
+      }
+    }
+
+    output.close();
+  }
+}
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..cc92ed0
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class SmartDashboardTest extends UtilityClassTest<SmartDashboard> {
+  private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("SmartDashboard");
+
+  SmartDashboardTest() {
+    super(SmartDashboard.class);
+  }
+
+  @BeforeEach
+  void beforeEach() {
+    m_table.getKeys().forEach(m_table::delete);
+  }
+
+  @Test
+  void getBadValueTest() {
+    assertEquals("Expected", SmartDashboard.getString("KEY_SHOULD_NOT_BE_FOUND", "Expected"));
+  }
+
+  @Test
+  void putStringTest() {
+    final String key = "putString";
+    final String value = "thisIsAValue";
+
+    SmartDashboard.putString(key, value);
+
+    assertEquals(value, m_table.getEntry(key).getString(""));
+  }
+
+  @Test
+  void getStringTest() {
+    final String key = "getString";
+    final String value = "thisIsAValue";
+
+    m_table.getEntry(key).setString(value);
+
+    assertEquals(value, SmartDashboard.getString(key, ""));
+  }
+
+  @Test
+  void putNumberTest() {
+    final String key = "PutNumber";
+    final int value = 2147483647;
+
+    SmartDashboard.putNumber(key, value);
+
+    assertEquals(value, m_table.getEntry(key).getNumber(0).intValue());
+  }
+
+  @Test
+  void getNumberTest() {
+    final String key = "GetNumber";
+    final int value = 2147483647;
+
+    m_table.getEntry(key).setNumber(value);
+
+    assertEquals(value, SmartDashboard.getNumber(key, 0), 0.01);
+  }
+
+  @Test
+  void putBooleanTest() {
+    final String key = "PutBoolean";
+    final boolean value = true;
+
+    SmartDashboard.putBoolean(key, value);
+
+    assertEquals(value, m_table.getEntry(key).getBoolean(!value));
+  }
+
+  @Test
+  void getBooleanTest() {
+    final String key = "GetBoolean";
+    final boolean value = true;
+
+    m_table.getEntry(key).setBoolean(value);
+
+    assertEquals(value, SmartDashboard.getBoolean(key, !value));
+  }
+
+  @Test
+  void testReplaceString() {
+    final String key = "testReplaceString";
+    final String valueNew = "newValue";
+
+    m_table.getEntry(key).setString("oldValue");
+    SmartDashboard.putString(key, valueNew);
+
+    assertEquals(valueNew, m_table.getEntry(key).getString(""));
+  }
+
+  @Test
+  void putStringNullKeyTest() {
+    assertThrows(NullPointerException.class,
+        () -> SmartDashboard.putString(null, "This should not work"));
+  }
+
+  @Test
+  void putStringNullValueTest() {
+    assertThrows(NullPointerException.class,
+        () -> SmartDashboard.putString("KEY_SHOULD_NOT_BE_STORED", null));
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
new file mode 100644
index 0000000..45a0638
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.spline;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+
+class CubicHermiteSplineTest {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
+  private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
+    // Start the timer.
+    var start = System.nanoTime();
+
+    // Generate and parameterize the spline.
+    var controlVectors =
+        SplineHelper.getCubicControlVectorsFromWaypoints(a,
+            waypoints.toArray(new Translation2d[0]), b);
+    var splines
+        = SplineHelper.getCubicSplinesFromControlVectors(
+        controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
+
+    var poses = new ArrayList<PoseWithCurvature>();
+
+    poses.add(splines[0].getPoint(0.0));
+
+    for (var spline : splines) {
+      poses.addAll(SplineParameterizer.parameterize(spline));
+    }
+
+    // End the timer.
+    var end = System.nanoTime();
+
+    // Calculate the duration (used when benchmarking)
+    var durationMicroseconds = (end - start) / 1000.0;
+
+    for (int i = 0; i < poses.size() - 1; i++) {
+      var p0 = poses.get(i);
+      var p1 = poses.get(i + 1);
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      var twist = p0.poseMeters.log(p1.poseMeters);
+      assertAll(
+          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
+      );
+    }
+
+    // Check first point
+    assertAll(
+        () -> assertEquals(a.getTranslation().getX(),
+            poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(a.getTranslation().getY(),
+            poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(a.getRotation().getRadians(),
+            poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
+    );
+
+    // Check interior waypoints
+    boolean interiorsGood = true;
+    for (var waypoint : waypoints) {
+      boolean found = false;
+      for (var state : poses) {
+        if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    assertTrue(interiorsGood);
+
+    // Check last point
+    assertAll(
+        () -> assertEquals(b.getTranslation().getX(),
+            poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(b.getTranslation().getY(),
+            poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(b.getRotation().getRadians(),
+            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
+    );
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testStraightLine() {
+    run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSCurve() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(1, 1));
+    waypoints.add(new Translation2d(2, -1));
+    var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
+
+    run(start, waypoints, end);
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testOneInterior() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(2.0, 0.0));
+    var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testWindyPath() {
+    final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    final ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(0.5, 0.5));
+    waypoints.add(new Translation2d(0.5, 0.5));
+    waypoints.add(new Translation2d(1.0, 0.0));
+    waypoints.add(new Translation2d(1.5, 0.5));
+    waypoints.add(new Translation2d(2.0, 0.0));
+    waypoints.add(new Translation2d(2.5, 0.5));
+    final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+        new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+        Arrays.asList(new Translation2d(10, 10.5)),
+        new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
new file mode 100644
index 0000000..1d65922
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.spline;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class QuinticHermiteSplineTest {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
+  private void run(Pose2d a, Pose2d b) {
+    // Start the timer.
+    var start = System.nanoTime();
+
+    // Generate and parameterize the spline.
+    var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
+    var spline = SplineHelper.getQuinticSplinesFromControlVectors(
+        controlVectors.toArray(new Spline.ControlVector[0]))[0];
+    var poses = SplineParameterizer.parameterize(spline);
+
+    // End the timer.
+    var end = System.nanoTime();
+
+    // Calculate the duration (used when benchmarking)
+    var durationMicroseconds = (end - start) / 1000.0;
+
+    for (int i = 0; i < poses.size() - 1; i++) {
+      var p0 = poses.get(i);
+      var p1 = poses.get(i + 1);
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      var twist = p0.poseMeters.log(p1.poseMeters);
+      assertAll(
+          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
+    }
+
+    // Check first point
+    assertAll(
+        () -> assertEquals(
+            a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(
+            a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(
+            a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
+            1E-9));
+
+    // Check last point
+    assertAll(
+        () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1)
+            .poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1)
+            .poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(b.getRotation().getRadians(),
+            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testStraightLine() {
+    run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSimpleSCurve() {
+    run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSquiggly() {
+    run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+        new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
+  }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+          new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
new file mode 100644
index 0000000..977c5ab
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class CentripetalAccelerationConstraintTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCentripetalAccelerationConstraint() {
+    double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
+    var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
+
+    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+        Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var centripetalAcceleration
+          = Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
+
+      t += dt;
+      assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
+    }
+  }
+
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
new file mode 100644
index 0000000..d025860
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDriveKinematicsConstraintTest {
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  @Test
+  void testDifferentialDriveKinematicsConstraint() {
+    double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
+    var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
+
+    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+        Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var chassisSpeeds = new ChassisSpeeds(
+          point.velocityMetersPerSecond, 0,
+          point.velocityMetersPerSecond * point.curvatureRadPerMeter
+      );
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      t += dt;
+      assertAll(
+          () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
+          () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
+      );
+    }
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
new file mode 100644
index 0000000..f5c6ff3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDriveVoltageConstraintTest {
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  @Test
+  void testDifferentialDriveVoltageConstraint() {
+    // Pick an unreasonably large kA to ensure the constraint has to do some work
+    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
+    var kinematics = new DifferentialDriveKinematics(0.5);
+    double maxVoltage = 10;
+    var constraint = new DifferentialDriveVoltageConstraint(feedforward,
+                                                            kinematics,
+                                                            maxVoltage);
+
+    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+        Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var chassisSpeeds = new ChassisSpeeds(
+          point.velocityMetersPerSecond, 0,
+          point.velocityMetersPerSecond * point.curvatureRadPerMeter
+      );
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      t += dt;
+
+      // Not really a strictly-correct test as we're using the chassis accel instead of the
+      // wheel accel, but much easier than doing it "properly" and a reasonable check anyway
+      assertAll(
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               <= maxVoltage + 0.05),
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               >= -maxVoltage - 0.05),
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               <= maxVoltage + 0.05),
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               >= -maxVoltage - 0.05)
+      );
+    }
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
new file mode 100644
index 0000000..81fb5b4
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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.trajectory;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class TrajectoryGeneratorTest {
+  static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
+    final double maxVelocity = feetToMeters(12.0);
+    final double maxAccel = feetToMeters(12);
+
+    // 2018 cross scale auto waypoints.
+    var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
+        Rotation2d.fromDegrees(-180));
+    var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
+        Rotation2d.fromDegrees(-160));
+
+    var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(sideStart);
+    waypoints.add(sideStart.plus(
+        new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
+            new Rotation2d())));
+    waypoints.add(sideStart.plus(
+        new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
+            Rotation2d.fromDegrees(-90))));
+    waypoints.add(crossScale);
+
+    TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
+        .setReversed(true)
+        .addConstraints(constraints);
+
+    return TrajectoryGenerator.generateTrajectory(waypoints, config);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  void testGenerationAndConstraints() {
+    Trajectory trajectory = getTrajectory(new ArrayList<>());
+
+    double duration = trajectory.getTotalTimeSeconds();
+    double t = 0.0;
+    double dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      t += dt;
+      assertAll(
+          () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
+          () -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
+              + 0.05)
+      );
+    }
+  }
+
+  @Test
+  void testMalformedTrajectory() {
+    var dsSim = new DriverStationSim();
+    dsSim.setSendError(false);
+
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+          Arrays.asList(
+            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+            new Pose2d(1, 0, Rotation2d.fromDegrees(180))
+          ),
+          new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
+        );
+
+    assertEquals(traj.getStates().size(), 1);
+    assertEquals(traj.getTotalTimeSeconds(), 0);
+
+    dsSim.setSendError(true);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
new file mode 100644
index 0000000..e389a4f
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class TrajectoryJsonTest {
+  @Test
+  void deserializeMatches() {
+    var config = List.of(new DifferentialDriveKinematicsConstraint(
+        new DifferentialDriveKinematics(20), 3));
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
+
+    var deserialized =
+        assertDoesNotThrow(() ->
+            TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
+
+    assertEquals(trajectory.getStates(), deserialized.getStates());
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
new file mode 100644
index 0000000..19f31dc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class TrajectoryTransformTest {
+  @Test
+  void testTransformBy() {
+    var config = new TrajectoryConfig(3, 3);
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
+        config
+    );
+
+    var transformedTrajectory = trajectory.transformBy(
+        new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
+
+    // Test initial pose.
+    assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
+        transformedTrajectory.sample(0).poseMeters);
+
+    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+  }
+
+  @Test
+  void testRelativeTo() {
+    var config = new TrajectoryConfig(3, 3);
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
+        List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
+        config
+    );
+
+    var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
+
+    // Test initial pose.
+    assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
+
+    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+  }
+
+  void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
+    for (int i = 0; i < statesA.size() - 1; i++) {
+      var a1 = statesA.get(i).poseMeters;
+      var a2 = statesA.get(i + 1).poseMeters;
+
+      var b1 = statesB.get(i).poseMeters;
+      var b2 = statesB.get(i + 1).poseMeters;
+
+      assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
+    }
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
new file mode 100644
index 0000000..090eacc
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
@@ -0,0 +1,257 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.trajectory;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@SuppressWarnings({"PMD.TooManyMethods", "PMD.AvoidInstantiatingObjectsInLoops"})
+class TrapezoidProfileTest {
+  private static final double kDt = 0.01;
+
+  /**
+   * Asserts "val1" is less than or equal to "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   */
+  private static void assertLessThanOrEquals(double val1, double val2) {
+    assertTrue(val1 <= val2, val1 + " is greater than " + val2);
+  }
+
+  /**
+   * Asserts "val1" is within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertNear(double val1, double val2, double eps) {
+    assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
+        + " is greater than " + eps);
+  }
+
+  /**
+   * Asserts "val1" is less than or within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertLessThanOrNear(double val1, double val2, double eps) {
+    if (val1 <= val2) {
+      assertLessThanOrEquals(val1, val2);
+    } else {
+      assertNear(val1, val2, eps);
+    }
+  }
+
+  @Test
+  void reachesGoal() {
+    TrapezoidProfile.Constraints constraints =
+        new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 450; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinousUnderVelChange() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double lastPos = state.position;
+    for (int i = 0; i < 1600; ++i) {
+      if (i == 400) {
+        constraints.maxVelocity = 0.75;
+      }
+
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      double estimatedVel = (state.position - lastPos) / kDt;
+
+      if (i >= 400) {
+        // Since estimatedVel can have floating point rounding errors, we check
+        // whether value is less than or within an error delta of the new
+        // constraint.
+        assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
+
+        assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
+      }
+
+      lastPos = state.position;
+    }
+    assertEquals(state, goal);
+  }
+
+  // There is some somewhat tricky code for dealing with going backwards
+  @Test
+  void backwards() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void switchGoalInMiddle() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNotEquals(state, goal);
+
+    goal = new TrapezoidProfile.State(0.0, 0.0);
+    for (int i = 0; i < 550; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Checks to make sure that it hits top speed
+  @Test
+  void topSpeed() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNear(constraints.maxVelocity, state.velocity, 10e-5);
+
+    for (int i = 0; i < 2000; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void timingToCurrent() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; i++) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
+    }
+  }
+
+  @Test
+  void timingToGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(1);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingToNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(-1);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java
new file mode 100644
index 0000000..839e2e3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class ErrorMessagesTest extends UtilityClassTest<ErrorMessages> {
+  ErrorMessagesTest() {
+    super(ErrorMessages.class);
+  }
+
+  @Test
+  void requireNonNullParamNullTest() {
+    assertThrows(NullPointerException.class, () -> requireNonNullParam(null, "testParam",
+        "testMethod"));
+  }
+
+  @Test
+  void requireNonNullParamNotNullTest() {
+    assertDoesNotThrow(() -> requireNonNullParam("null", "testParam", "testMethod"));
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
new file mode 100644
index 0000000..1d00046
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class UnitsTest extends UtilityClassTest<Units> {
+  UnitsTest() {
+    super(Units.class);
+  }
+
+  @Test
+  void metersToFeetTest() {
+    assertEquals(3.28, Units.metersToFeet(1), 1e-2);
+  }
+
+  @Test
+  void feetToMetersTest() {
+    assertEquals(0.30, Units.feetToMeters(1), 1e-2);
+  }
+
+  @Test
+  void metersToInchesTest() {
+    assertEquals(39.37, Units.metersToInches(1), 1e-2);
+  }
+
+  @Test
+  void inchesToMetersTest() {
+    assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
+  }
+
+  @Test
+  void degreesToRadiansTest() {
+    assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
+  }
+
+  @Test
+  void radiansToDegreesTest() {
+    assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
+  }
+
+  @Test
+  void rotationsPerMinuteToRadiansPerSecondTest() {
+    assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
+  }
+
+  @Test
+  void radiansPerSecondToRotationsPerMinute() {
+    assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
+  }
+}
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
diff --git a/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini b/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
new file mode 100644
index 0000000..a685de0
--- /dev/null
+++ b/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
@@ -0,0 +1,7 @@
+[NetworkTables Storage 3.0]
+double "/Preferences/checkedValueInt"=2
+double "/Preferences/checkedValueDouble"=.2
+double "/Preferences/checkedValueFloat"=3.14
+double "/Preferences/checkedValueLong"=172
+string "/Preferences/checkedValueString"="Hello. How are you?"
+boolean "/Preferences/checkedValueBoolean"=false