Initial stab at the new WPILib.

Change-Id: Id04cc07649959566deb5b4fa637267072a5191ca
diff --git a/WORKSPACE b/WORKSPACE
index 1a5bff3..e1cb1f8 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -16,8 +16,8 @@
 new_http_archive(
   name = 'arm_frc_linux_gnueabi_repo',
   build_file = 'tools/cpp/arm-frc-linux-gnueabi/arm-frc-linux-gnueabi.BUILD',
-  sha256 = '9e2e58f0a668c22e46486a76df8b9da08f526cd8bf4e579f19b461f70a358bf8',
-  url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2017.tar.xz',
+  sha256 = '875b23bec5138e09e3d21cc1ff2727ea3ecbec57509c37589514ba50f92979c7',
+  url = 'http://frc971.org/Build-Dependencies/roborio-compiler-2018.tar.xz',
 )
 
 # Recompressed version of the one downloaded from Linaro at
@@ -57,6 +57,14 @@
 )
 
 new_http_archive(
+  name = 'allwpilib_ni_libraries_repo_2018',
+  build_file = 'debian/ni-libraries-2018.BUILD',
+  sha256 = '05ef6701c77b83163b443aa956d151028861cc3fa29fdf2b6b77431b4a91bfb9',
+  url = 'http://frc971.org/Build-Dependencies/allwpilib_ni-libraries_57e9fb3.tar.gz',
+  strip_prefix = 'ni-libraries',
+)
+
+new_http_archive(
   name = 'allwpilib_ni_libraries_repo_2017',
   build_file = 'debian/ni-libraries-2017.BUILD',
   sha256 = '67c1ad365fb712cc0acb0bf43465b831030523dc6f88daa02626994f644d91eb',
diff --git a/debian/ni-libraries-2018.BUILD b/debian/ni-libraries-2018.BUILD
new file mode 100644
index 0000000..ca17626
--- /dev/null
+++ b/debian/ni-libraries-2018.BUILD
@@ -0,0 +1,19 @@
+cc_library(
+    name = "ni-libraries",
+    srcs = [
+        "lib/libFRC_NetworkCommunication.so.18.0.0",
+        "lib/libNiFpga.so.17.0.0",
+        "lib/libNiFpgaLv.so.17.0.0",
+        "lib/libNiRioSrv.so.17.0.0",
+        "lib/libRoboRIO_FRC_ChipObject.so.18.0.0",
+        "lib/libniriodevenum.so.17.0.0",
+        "lib/libniriosession.so.17.0.0",
+    ],
+    hdrs = glob(["include/**"]),
+    includes = [
+        "include",
+    ],
+    linkstatic = True,
+    restricted_to = ["@//tools:roborio"],
+    visibility = ["//visibility:public"],
+)
diff --git a/frc971/wpilib/dma.cc b/frc971/wpilib/dma.cc
index dbbfc56..8dca72c 100644
--- a/frc971/wpilib/dma.cc
+++ b/frc971/wpilib/dma.cc
@@ -8,9 +8,7 @@
 #include "DigitalSource.h"
 #include "AnalogInput.h"
 #include "Encoder.h"
-#ifdef WPILIB2017
 #include "HAL/HAL.h"
-#endif
 
 // Interface to the roboRIO FPGA's DMA features.
 
@@ -30,10 +28,6 @@
 static constexpr ssize_t kChannelSize[20] = {2, 2, 4, 4, 2, 2, 4, 4, 3, 3,
                                              2, 1, 4, 4, 4, 4, 4, 4, 4, 4};
 
-#ifndef WPILIB2017
-#define HAL_GetErrorMessage getHALErrorMessage
-#endif
-
 enum DMAOffsetConstants {
   kEnable_AI0_Low = 0,
   kEnable_AI0_High = 1,
@@ -188,12 +182,7 @@
   new_trigger.RisingEdge = rising;
   new_trigger.ExternalClockSource_AnalogTrigger = false;
   unsigned char module = 0;
-  uint32_t channel =
-#ifdef WPILIB2017
-      input->GetChannel();
-#else
-      input->GetChannelForRouting();
-#endif
+  uint32_t channel = input->GetChannel();
   if (channel >= kNumHeaders) {
     module = 1;
     channel -= kNumHeaders;
@@ -228,6 +217,7 @@
   sample->dma_ = this;
   manager_->read(sample->read_buffer_, capture_size_, timeout_ms,
                  &remainingBytes, &status);
+  sample->CalculateTimestamp();
 
   // TODO(jerry): Do this only if status == 0?
   *remaining_out = remainingBytes / capture_size_;
@@ -296,7 +286,7 @@
   }
 
   manager_.reset(
-      new nFPGA::tDMAManager(0, queue_depth * capture_size_, &status));
+      new nFPGA::tDMAManager(1, queue_depth * capture_size_, &status));
 
   wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
   if (status != 0) {
@@ -323,10 +313,23 @@
 
 static_assert(::std::is_pod<DMASample>::value, "DMASample needs to be POD");
 
-ssize_t DMASample::offset(int index) const { return dma_->channel_offsets_[index]; }
+ssize_t DMASample::offset(int index) const {
+  return dma_->channel_offsets_[index];
+}
 
-uint32_t DMASample::GetTime() const {
-  return read_buffer_[dma_->capture_size_ - 1];
+void DMASample::CalculateTimestamp() {
+  uint32_t lower_sample = read_buffer_[dma_->capture_size_ - 1];
+#if WPILIB2018
+  int32_t status = 0;
+  fpga_timestamp_ = HAL_ExpandFPGATime(lower_sample, &status);
+  assert(status == 0);
+#else
+  fpga_timestamp_ = lower_sample;
+#endif
+}
+
+uint64_t DMASample::GetTime() const {
+  return fpga_timestamp_;
 }
 
 double DMASample::GetTimestamp() const {
@@ -340,12 +343,7 @@
         HAL_GetErrorMessage(NiFpga_Status_ResourceNotFound));
     return false;
   }
-  const uint32_t channel =
-#ifdef WPILIB2017
-      input->GetChannel();
-#else
-      input->GetChannelForRouting();
-#endif
+  const uint32_t channel = input->GetChannel();
   if (channel < kNumHeaders) {
     return (read_buffer_[offset(kEnable_DI)] >> channel) & 0x1;
   } else {
diff --git a/frc971/wpilib/dma.h b/frc971/wpilib/dma.h
index e07ab36..0869b0d 100644
--- a/frc971/wpilib/dma.h
+++ b/frc971/wpilib/dma.h
@@ -9,7 +9,7 @@
 #include <array>
 #include <memory>
 
-#ifdef WPILIB2017
+#if defined(WPILIB2017) || defined(WPILIB2018)
 #include "HAL/ChipObject.h"
 #else
 #include "ChipObject.h"
@@ -17,7 +17,7 @@
 #include "ErrorBase.h"
 
 class DMA;
-#ifdef WPILIB2017
+#if defined(WPILIB2017) || defined(WPILIB2018)
 namespace frc {
 class DigitalSource;
 class AnalogInput;
@@ -43,7 +43,7 @@
   // Returns the FPGA timestamp of the sample in seconds.
   double GetTimestamp() const;
   // Returns the FPGA timestamp of the sample in microseconds.
-  uint32_t GetTime() const;
+  uint64_t GetTime() const;
 
   // All Get methods either return the requested value, or set the Error.
 
@@ -61,6 +61,8 @@
  private:
   friend DMA;
 
+  void CalculateTimestamp();
+
   // Returns the offset of the sample type in the buffer, or -1 if it isn't in
   // the sample.
   ssize_t offset(int index) const;
@@ -69,6 +71,7 @@
   // into WPILib.
 
   DMA *dma_;
+  uint64_t fpga_timestamp_;
   uint32_t read_buffer_[64];
 };
 
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index afb880f..8487ce9 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -17,11 +17,9 @@
 
   if (!previous_value && ExtractValue(sample)) {
     pos_edge_count_++;
-    pos_edge_time_ = sample.GetTimestamp();
     pos_last_encoder_ = sample.GetRaw(encoder_);
   } else if (previous_value && !ExtractValue(sample)) {
     neg_edge_count_++;
-    neg_edge_time_ = sample.GetTimestamp();
     neg_last_encoder_ = sample.GetRaw(encoder_);
   }
 }
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index 64722ca..c71e7ae 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -90,12 +90,10 @@
 
   // Values related to the positive edge.
   int pos_edge_count_ = 0;
-  double pos_edge_time_ = 0.0;
   int pos_last_encoder_ = 0;
 
   // Values related to the negative edge.
   int neg_edge_count_ = 0;
-  double neg_edge_time_ = 0.0;
   int neg_last_encoder_ = 0;
 
   bool polled_value_ = false;
diff --git a/frc971/wpilib/interrupt_edge_counting.cc b/frc971/wpilib/interrupt_edge_counting.cc
index ec6f627..3f81501 100644
--- a/frc971/wpilib/interrupt_edge_counting.cc
+++ b/frc971/wpilib/interrupt_edge_counting.cc
@@ -21,11 +21,7 @@
 
 void EdgeCounter::operator()() {
   ::aos::SetCurrentThreadName("EdgeCounter_" +
-#ifdef WPILIB2017
                               ::std::to_string(input_->GetChannel()));
-#else
-                              ::std::to_string(input_->GetChannelForRouting()));
-#endif
 
   input_->RequestInterrupts();
   input_->SetUpSourceEdge(true, true);
@@ -59,11 +55,7 @@
       current_value_ = hall_value;
     } else {
       LOG(WARNING, "Detected spurious edge on %d.  Dropping it.\n",
-#ifdef WPILIB2017
           input_->GetChannel());
-#else
-          input_->GetChannelForRouting());
-#endif
     }
   }
 }
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index a490e67..7d406d0 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -6,7 +6,7 @@
 #include "aos/common/logging/queue_logging.h"
 
 #include "DriverStation.h"
-#ifdef WPILIB2017
+#if defined(WPILIB2017) || defined(WPILIB2018)
 #include "HAL/HAL.h"
 #else
 #include "HAL/HAL.hpp"
@@ -31,7 +31,7 @@
     ds->WaitForData();
     auto new_state = ::aos::joystick_state.MakeMessage();
 
-#ifdef WPILIB2017
+#if defined(WPILIB2017) || defined(WPILIB2018)
     HAL_ControlWord control_word;
     HAL_GetControlWord(&control_word);
 #else
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index af72f7d..c886907 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -3,37 +3,27 @@
 #include "aos/common/messages/robot_state.q.h"
 #include "aos/common/logging/queue_logging.h"
 
-#include "DriverStation.h"
-#include "ControllerPower.h"
-#undef ERROR
-
-#ifndef WPILIB2017
-namespace frc {
-using ::DriverStation;
-}  // namespace frc
-#endif
+#include <HAL/HAL.h>
 
 namespace frc971 {
 namespace wpilib {
 
-void SendRobotState(int32_t my_pid, frc::DriverStation *ds) {
+void SendRobotState(int32_t my_pid) {
   auto new_state = ::aos::robot_state.MakeMessage();
 
+  int32_t status = 0;
+
   new_state->reader_pid = my_pid;
-  new_state->outputs_enabled = ds->IsSysActive();
-#ifdef WPILIB2017
-  new_state->browned_out = ds->IsBrownedOut();
-#else
-  new_state->browned_out = ds->IsSysBrownedOut();
-#endif
+  new_state->outputs_enabled = HAL_GetSystemActive(&status);
+  new_state->browned_out = HAL_GetBrownedOut(&status);
 
-  new_state->is_3v3_active = ControllerPower::GetEnabled3V3();
-  new_state->is_5v_active = ControllerPower::GetEnabled5V();
-  new_state->voltage_3v3 = ControllerPower::GetVoltage3V3();
-  new_state->voltage_5v = ControllerPower::GetVoltage5V();
+  new_state->is_3v3_active = HAL_GetUserActive3V3(&status);
+  new_state->is_5v_active = HAL_GetUserActive5V(&status);
+  new_state->voltage_3v3 = HAL_GetUserVoltage3V3(&status);
+  new_state->voltage_5v = HAL_GetUserVoltage5V(&status);
 
-  new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
-  new_state->voltage_battery = ds->GetBatteryVoltage();
+  new_state->voltage_roborio_in = HAL_GetVinVoltage(&status);
+  new_state->voltage_battery = HAL_GetVinVoltage(&status);
 
   LOG_STRUCT(DEBUG, "robot_state", *new_state);
 
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 1c55cc7..100c67e 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -3,22 +3,11 @@
 
 #include <stdint.h>
 
-#ifdef WPILIB2017
-namespace frc {
-class DriverStation;
-}  // namespace frc
-#else
-class DriverStation;
-namespace frc {
-using ::DriverStation;
-}  // namespace frc
-#endif
-
 namespace frc971 {
 namespace wpilib {
 
 // Sends out a message on ::aos::robot_state.
-void SendRobotState(int32_t my_pid, ::frc::DriverStation *ds);
+void SendRobotState(int32_t my_pid);
 
 }  // namespace wpilib
 }  // namespace frc971
diff --git a/motors/algorithms_test.cc b/motors/algorithms_test.cc
index 87cb5b5..32ca8b7 100644
--- a/motors/algorithms_test.cc
+++ b/motors/algorithms_test.cc
@@ -1,6 +1,7 @@
 #include "motors/algorithms.h"
 
 #include <inttypes.h>
+#include <cmath>
 
 #include "gtest/gtest.h"
 
diff --git a/setup_roborio.sh b/setup_roborio.sh
index f8e9bbf..d7a4144 100755
--- a/setup_roborio.sh
+++ b/setup_roborio.sh
@@ -24,11 +24,6 @@
   ssh "admin@${ROBOT_HOSTNAME}" 'echo "alias l=\"ls -la\"" >> /etc/profile'
 fi
 
-# Make sure starter.sh has the correct permissions to run the robot code.
-# If missing o+rx, the robot code will not start.  No error messages on
-# some driver stations.
-ssh "admin@${ROBOT_HOSTNAME}" 'chmod go+rx robot_code/starter.sh'
-
 ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t'
 
 echo "Deploying robotCommand startup script"
diff --git a/third_party/BUILD b/third_party/BUILD
index 3f7352f..20bb7e0 100644
--- a/third_party/BUILD
+++ b/third_party/BUILD
@@ -1,6 +1,6 @@
 cc_library(
   name = 'wpilib',
-  deps = ['//third_party/allwpilib_2017:wpilib'],
+  deps = ['//third_party/allwpilib_2018:wpilib'],
   visibility = ['//visibility:public'],
   linkstatic = True,
   restricted_to = ['//tools:roborio'],
diff --git a/third_party/allwpilib_2018/BUILD b/third_party/allwpilib_2018/BUILD
new file mode 100644
index 0000000..8254c8a
--- /dev/null
+++ b/third_party/allwpilib_2018/BUILD
@@ -0,0 +1,143 @@
+licenses(['notice'])
+
+genrule(
+  name = 'wpilib_version',
+  outs = ['shared/src/WPILibVersion.cpp'],
+  cmd = '\n'.join([
+    "cat > \"$@\" << EOF",
+    "// Autogenerated file! Do not manually edit this file.",
+    "#include \"WPILibVersion.h\"",
+    "const char* GetWPILibVersion() {",
+    "  return \"2018-frc971\";",
+    "}",
+    "EOF",
+  ]),
+)
+
+_header_dirs = [
+  'wpilibc/src/main/native/include',
+  #'wpilibc/shared/include',
+  #'wpilibc/athena/include',
+  'hal/src/main/native/include',
+  'hal/src/main/native/athena',
+  'hal/src/main/native/include/HAL/cpp',
+  #'hal/include',
+  #'hal/lib/athena',
+]
+
+# Names of WPILib "devices" I don't want to deal with making trivial updates to
+# chop out various ugliness or have to vet for sanity.
+_excluded_devices = [
+  'ADXL345_I2C',
+  'ADXL345_SPI',
+  'ADXL362',
+  'ADXRS450_Gyro',
+  'AnalogAccelerometer',
+  #'AnalogGyro',
+  'AnalogPotentiometer',
+  'CANJaguar',
+  'CANSpeedController',
+  'CANTalon',
+  'CameraServer',
+  'DoubleSolenoid',
+  'GamepadBase',
+  'GearTooth',
+  'GenericHID',
+  #'GyroBase',
+  'IterativeRobot',
+  'Jaguar',
+  'Joystick',
+  'JoystickBase',
+  #'Log',
+  #'MotorSafety',
+  #'MotorSafetyHelper',
+  'OSSerialPort',
+  #'PIDController',
+  #'PIDSource',
+  #'PWMSpeedController',
+  'Potentiometer',
+  'RobotDrive',
+  'SD540',
+  #'SafePWM',
+  #'SerialHelper',
+  #'SerialPort',
+  'Spark',
+  #'SpeedController',
+  'TalonSRX',
+  'Ultrasonic',
+  'Victor',
+  'XboxController',
+  #'visa',
+]
+
+# Whole subdirectories of WPILib we don't want around.
+_excluded_directories = [
+  #'SmartDashboard',
+  #'LiveWindow',
+  #'Commands',
+  #'Buttons',
+  #'Filters',
+  'LabView',
+  'vision',
+]
+
+# Header files we don't want to have.
+_bad_hdrs = ([
+  'wpilibc/src/main/native/include/WPILib.h',
+  'hal/src/main/native/include/HAL/LabVIEW/HAL.h',
+  #'wpilibc/**/Accelerometer.*',
+] + ['**/%s/**' % d for d in _excluded_directories] +
+ ['**/%s.*' % d for d in _excluded_devices])
+_h_hdrs = glob([d + '/**/*.h' for d in _header_dirs], exclude=_bad_hdrs)
+_hpp_hdrs = glob([d + '/**/*.hpp' for d in _header_dirs], exclude=_bad_hdrs)
+
+cc_library(
+  name = 'wpilib',
+  visibility = ['//third_party:__pkg__'],
+  srcs = glob([
+    'wpilibc/src/main/native/cpp/*.cpp',
+    'wpilibc/src/main/native/cpp/interfaces/*.cpp',
+    'wpilibc/src/main/native/cpp/LiveWindow/*.cpp',
+    'wpilibc/src/main/native/cpp/Commands/*.cpp',
+    'wpilibc/src/main/native/cpp/SmartDashboard/*.cpp',
+
+    'hal/src/main/native/athena/*.cpp',
+    'hal/src/main/native/athena/cpp/*.cpp',
+    'hal/src/main/native/athena/ctre/*.cpp',
+    'hal/src/main/native/shared/handles/*.cpp',
+    'wpilibc/src/main/native/cpp/Internal/*.cpp',
+  ], exclude = (
+    ['**/%s/**' % d for d in _excluded_directories] +
+    ['**/%s.*' % d for d in _excluded_devices] + [
+    #'wpilibc/**/Accelerometer.*',
+  ])) + [
+    ':wpilib_version',
+  ],
+  copts = [
+    '-Wno-unused-parameter',
+    '-Wno-switch-enum',
+    '-Wno-attributes',
+    '-Wno-cast-align',
+    '-Wno-cast-qual',
+    '-Wno-deprecated-declarations',
+    '-Wno-error',
+    #'-Wno-unused-const-variable',
+  ],
+  deps = [
+    '//third_party/ntcore_2018:ntcore',
+    '@allwpilib_ni_libraries_repo_2018//:ni-libraries',
+    '//aos/common/logging',
+  ],
+  hdrs = _h_hdrs + _hpp_hdrs + [
+    'wpilibc/src/main/native/include/circular_buffer.inc',
+    'wpilibc/src/main/native/include/SpeedControllerGroup.inc',
+  ],
+  includes = _header_dirs,
+  linkopts = [
+    '-lpthread',
+  ],
+  defines = [
+    'WPILIB2018=1',
+  ],
+  restricted_to = ['//tools:roborio'],
+)
diff --git a/third_party/allwpilib_2018/hal/src/main/native/athena/HAL.cpp b/third_party/allwpilib_2018/hal/src/main/native/athena/HAL.cpp
index 4c65597..faa88b5 100644
--- a/third_party/allwpilib_2018/hal/src/main/native/athena/HAL.cpp
+++ b/third_party/allwpilib_2018/hal/src/main/native/athena/HAL.cpp
@@ -61,7 +61,7 @@
   InitializeI2C();
   InitialzeInterrupts();
   InitializeNotifier();
-  InitializeOSSerialPort();
+  //InitializeOSSerialPort();
   InitializePCMInternal();
   InitializePDP();
   InitializePorts();
@@ -272,6 +272,40 @@
 }
 
 /**
+ * Convert a freestanding lower half to a 64 bit FPGA timestamp
+ *
+ * Note: This is making the assumption that the timestamp being converted is
+ * always in the past.  If you call this with a future timestamp, it probably
+ * will make it in the past.  If you wait over 70 minutes between capturing the
+ * bottom 32 bits of the timestamp and expanding it, you will be off by
+ * multiples of 1<<32 microseconds.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset) as a 64 bit number.
+ */
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+  // Capture the current FPGA time.  This will give us the upper half of the
+  // clock.
+  uint64_t fpga_time = HAL_GetFPGATime(status);
+  if (*status != 0) return 0;
+
+  // Now, we need to detect the case where the lower bits rolled over after we
+  // sampled.  In that case, the upper bits will be 1 bigger than they should
+  // be.
+
+  // Break it into lower and upper portions.
+  uint32_t lower = fpga_time & ((uint64_t)0xffffffff);
+  uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+
+  // The time was sampled *before* the current time, so roll it back.
+  if (lower < unexpanded_lower) {
+    --upper;
+  }
+
+  return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+}
+
+/**
  * Get the state of the "USER" button on the roboRIO
  * @return true if the button is currently pressed down
  */
diff --git a/third_party/allwpilib_2018/hal/src/main/native/athena/Interrupts.cpp b/third_party/allwpilib_2018/hal/src/main/native/athena/Interrupts.cpp
index afc6ec0..c8c38df 100644
--- a/third_party/allwpilib_2018/hal/src/main/native/athena/Interrupts.cpp
+++ b/third_party/allwpilib_2018/hal/src/main/native/athena/Interrupts.cpp
@@ -14,6 +14,7 @@
 #include "DigitalInternal.h"
 #include "HAL/ChipObject.h"
 #include "HAL/Errors.h"
+#include "HAL/HAL.h"
 #include "HAL/cpp/make_unique.h"
 #include "HAL/handles/HandlesInternal.h"
 #include "HAL/handles/LimitedHandleResource.h"
@@ -174,9 +175,9 @@
 /**
  * Return the timestamp for the rising interrupt that occurred most recently.
  * This is in the same time domain as GetClock().
- * @return Timestamp in seconds since boot.
+ * @return Timestamp in microseconds since boot.
  */
-double HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
+uint64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
                                         int32_t* status) {
   auto anInterrupt = interruptHandles->Get(interruptHandle);
   if (anInterrupt == nullptr) {
@@ -184,7 +185,7 @@
     return 0;
   }
   uint32_t timestamp = anInterrupt->anInterrupt->readRisingTimeStamp(status);
-  return timestamp * 1e-6;
+  return HAL_ExpandFPGATime(timestamp, status);
 }
 
 /**
@@ -192,7 +193,7 @@
  * This is in the same time domain as GetClock().
  * @return Timestamp in seconds since boot.
  */
-double HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
+uint64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
                                          int32_t* status) {
   auto anInterrupt = interruptHandles->Get(interruptHandle);
   if (anInterrupt == nullptr) {
@@ -200,7 +201,7 @@
     return 0;
   }
   uint32_t timestamp = anInterrupt->anInterrupt->readFallingTimeStamp(status);
-  return timestamp * 1e-6;
+  return HAL_ExpandFPGATime(timestamp, status);
 }
 
 void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
diff --git a/third_party/allwpilib_2018/hal/src/main/native/include/HAL/HAL.h b/third_party/allwpilib_2018/hal/src/main/native/include/HAL/HAL.h
index 2b765e5..ea396ca 100644
--- a/third_party/allwpilib_2018/hal/src/main/native/include/HAL/HAL.h
+++ b/third_party/allwpilib_2018/hal/src/main/native/include/HAL/HAL.h
@@ -68,6 +68,7 @@
 HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel);
 
 uint64_t HAL_GetFPGATime(int32_t* status);
+uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t *status);
 
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
 
diff --git a/third_party/allwpilib_2018/hal/src/main/native/include/HAL/Interrupts.h b/third_party/allwpilib_2018/hal/src/main/native/include/HAL/Interrupts.h
index db522f9..dbd7200 100644
--- a/third_party/allwpilib_2018/hal/src/main/native/include/HAL/Interrupts.h
+++ b/third_party/allwpilib_2018/hal/src/main/native/include/HAL/Interrupts.h
@@ -28,10 +28,10 @@
 void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle, int32_t* status);
 void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
                            int32_t* status);
-double HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
-                                        int32_t* status);
-double HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
-                                         int32_t* status);
+uint64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
+                                          int32_t *status);
+uint64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
+                                           int32_t *status);
 void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
                            HAL_Handle digitalSourceHandle,
                            HAL_AnalogTriggerType analogTriggerType,
diff --git a/third_party/allwpilib_2018/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/third_party/allwpilib_2018/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 5c9d448..0a11bbe 100644
--- a/third_party/allwpilib_2018/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/third_party/allwpilib_2018/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -152,10 +152,23 @@
  * @return Timestamp in seconds since boot.
  */
 double InterruptableSensorBase::ReadRisingTimestamp() {
+  return static_cast<double>(ReadRisingTimestampMicroseconds()) * 1e-6;
+}
+
+/**
+ * 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 #DigitalInput.SetUpSourceEdge}
+ *
+ * @return Timestamp in microseconds since boot.
+ */
+uint64_t InterruptableSensorBase::ReadRisingTimestampMicroseconds() {
   if (StatusIsFatal()) return 0.0;
   wpi_assert(m_interrupt != HAL_kInvalidHandle);
   int32_t status = 0;
-  double timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
+  uint64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
   wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
   return timestamp;
 }
@@ -170,10 +183,23 @@
  * @return Timestamp in seconds since boot.
  */
 double InterruptableSensorBase::ReadFallingTimestamp() {
+  return static_cast<double>(ReadFallingTimestampMicroseconds()) * 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 #DigitalInput.SetUpSourceEdge}
+ *
+ * @return Timestamp in microseconds since boot.
+ */
+uint64_t InterruptableSensorBase::ReadFallingTimestampMicroseconds() {
   if (StatusIsFatal()) return 0.0;
   wpi_assert(m_interrupt != HAL_kInvalidHandle);
   int32_t status = 0;
-  double timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
+  uint64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
   wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
   return timestamp;
 }
diff --git a/third_party/allwpilib_2018/wpilibc/src/main/native/include/InterruptableSensorBase.h b/third_party/allwpilib_2018/wpilibc/src/main/native/include/InterruptableSensorBase.h
index 267353a..4d5c1ba 100644
--- a/third_party/allwpilib_2018/wpilibc/src/main/native/include/InterruptableSensorBase.h
+++ b/third_party/allwpilib_2018/wpilibc/src/main/native/include/InterruptableSensorBase.h
@@ -50,9 +50,11 @@
 
   // Return the timestamp for the rising interrupt that occurred.
   virtual double ReadRisingTimestamp();
+  virtual uint64_t ReadRisingTimestampMicroseconds();
 
   // Return the timestamp for the falling interrupt that occurred.
   virtual double ReadFallingTimestamp();
+  virtual uint64_t ReadFallingTimestampMicroseconds();
 
   virtual void SetUpSourceEdge(bool risingEdge, bool fallingEdge);
 
diff --git a/third_party/allwpilib_2018/wpilibc/src/main/native/include/Utility.h b/third_party/allwpilib_2018/wpilibc/src/main/native/include/Utility.h
index 616f4e3..007f8be 100644
--- a/third_party/allwpilib_2018/wpilibc/src/main/native/include/Utility.h
+++ b/third_party/allwpilib_2018/wpilibc/src/main/native/include/Utility.h
@@ -59,7 +59,7 @@
 int GetFPGAVersion();
 WPI_DEPRECATED("Use RobotController static class method")
 int64_t GetFPGARevision();
-WPI_DEPRECATED("Use RobotController static class method")
+//WPI_DEPRECATED("Use RobotController static class method")
 uint64_t GetFPGATime();
 WPI_DEPRECATED("Use RobotController static class method")
 bool GetUserButton();
diff --git a/third_party/allwpilib_2018/wpilibj/src/main/native/cpp/InterruptJNI.cpp b/third_party/allwpilib_2018/wpilibj/src/main/native/cpp/InterruptJNI.cpp
index 6150116..dc17f7b 100644
--- a/third_party/allwpilib_2018/wpilibj/src/main/native/cpp/InterruptJNI.cpp
+++ b/third_party/allwpilib_2018/wpilibj/src/main/native/cpp/InterruptJNI.cpp
@@ -232,7 +232,9 @@
   INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Handle = " << (HAL_InterruptHandle)interruptHandle;
 
   int32_t status = 0;
-  jdouble timeStamp = HAL_ReadInterruptRisingTimestamp((HAL_InterruptHandle)interruptHandle, &status);
+  jdouble timeStamp = static_cast<jdouble>(HAL_ReadInterruptRisingTimestamp(
+                          (HAL_InterruptHandle)interruptHandle, &status)) *
+                      1e-6;
 
   INTERRUPTJNI_LOG(logDEBUG) << "Status = " << status;
   CheckStatus(env, status);
diff --git a/third_party/gperftools/BUILD b/third_party/gperftools/BUILD
index 3fa0ab3..3310c78 100644
--- a/third_party/gperftools/BUILD
+++ b/third_party/gperftools/BUILD
@@ -5,6 +5,7 @@
 
 common_copts = [
   # Stuff from their Makefile.
+  '-Wno-cast-align',
   '-Wno-sign-compare',
   '-fno-builtin-malloc',
   '-fno-builtin-free',
diff --git a/third_party/ntcore_2018/BUILD b/third_party/ntcore_2018/BUILD
new file mode 100644
index 0000000..4a6341c
--- /dev/null
+++ b/third_party/ntcore_2018/BUILD
@@ -0,0 +1,31 @@
+licenses(['notice'])
+
+cc_library(
+  name = 'ntcore',
+  visibility = ['//visibility:public'],
+  srcs = glob([
+    'src/main/**/*.cpp',
+    'src/main/**/*.h',
+  ], exclude = [
+    #'src/main/native/cpp/networktables/**',
+    'src/main/native/cpp/jni/**',
+  ]),
+  copts = [
+    '-Wno-switch-enum',
+    '-Wno-cast-align',
+  ],
+  hdrs = glob([
+    'src/main/native/include/**/*.h',
+    'src/main/native/include/**/*.inl',
+  ]),
+  includes = [
+    'src/main/native/include',
+  ],
+  linkopts = [
+    '-lpthread',
+  ],
+  deps = [
+    '//third_party/wpiutil_2018:wpiutil',
+  ],
+  restricted_to = ['//tools:roborio'],
+)
diff --git a/third_party/ntcore_2018/src/main/native/cpp/ConnectionNotifier.h b/third_party/ntcore_2018/src/main/native/cpp/ConnectionNotifier.h
index 4d3cc92..4950ae3 100644
--- a/third_party/ntcore_2018/src/main/native/cpp/ConnectionNotifier.h
+++ b/third_party/ntcore_2018/src/main/native/cpp/ConnectionNotifier.h
@@ -22,8 +22,8 @@
  public:
   explicit ConnectionNotifierThread(int inst) : m_inst(inst) {}
 
-  bool Matches(const ListenerData& listener,
-               const ConnectionNotification& data) {
+  bool Matches(const ListenerData& /*listener*/,
+               const ConnectionNotification& /*data*/) {
     return true;
   }
 
diff --git a/third_party/ntcore_2018/src/main/native/cpp/RpcServer.h b/third_party/ntcore_2018/src/main/native/cpp/RpcServer.h
index 2b80db7..347c7d5 100644
--- a/third_party/ntcore_2018/src/main/native/cpp/RpcServer.h
+++ b/third_party/ntcore_2018/src/main/native/cpp/RpcServer.h
@@ -44,11 +44,11 @@
   RpcServerThread(int inst, wpi::Logger& logger)
       : m_inst(inst), m_logger(logger) {}
 
-  bool Matches(const RpcListenerData& listener, const RpcNotifierData& data) {
+  bool Matches(const RpcListenerData& /*listener*/, const RpcNotifierData& data) {
     return !data.name.empty() && data.send_response;
   }
 
-  void SetListener(RpcNotifierData* data, unsigned int listener_uid) {
+  void SetListener(RpcNotifierData* data, unsigned int /*listener_uid*/) {
     unsigned int local_id = Handle{data->entry}.GetIndex();
     unsigned int call_uid = Handle{data->call}.GetIndex();
     RpcIdPair lookup_uid{local_id, call_uid};
diff --git a/third_party/ntcore_2018/src/main/native/cpp/Storage.cpp b/third_party/ntcore_2018/src/main/native/cpp/Storage.cpp
index 8bb37f8..e291458 100644
--- a/third_party/ntcore_2018/src/main/native/cpp/Storage.cpp
+++ b/third_party/ntcore_2018/src/main/native/cpp/Storage.cpp
@@ -310,7 +310,7 @@
 }
 
 void Storage::ProcessIncomingExecuteRpc(
-    std::shared_ptr<Message> msg, INetworkConnection* conn,
+    std::shared_ptr<Message> msg, INetworkConnection* /*conn*/,
     std::weak_ptr<INetworkConnection> conn_weak) {
   std::unique_lock<wpi::mutex> lock(m_mutex);
   if (!m_server) return;  // only process on server
@@ -350,7 +350,7 @@
 }
 
 void Storage::ProcessIncomingRpcResponse(std::shared_ptr<Message> msg,
-                                         INetworkConnection* conn) {
+                                         INetworkConnection* /*conn*/) {
   std::unique_lock<wpi::mutex> lock(m_mutex);
   if (m_server) return;  // only process on client
   unsigned int id = msg->id();
@@ -387,7 +387,7 @@
 
 void Storage::ApplyInitialAssignments(
     INetworkConnection& conn, llvm::ArrayRef<std::shared_ptr<Message>> msgs,
-    bool new_server, std::vector<std::shared_ptr<Message>>* out_msgs) {
+    bool /*new_server*/, std::vector<std::shared_ptr<Message>>* out_msgs) {
   std::unique_lock<wpi::mutex> lock(m_mutex);
   if (m_server) return;  // should not do this on server
 
diff --git a/third_party/ntcore_2018/src/main/native/cpp/ntcore_c.cpp b/third_party/ntcore_2018/src/main/native/cpp/ntcore_c.cpp
index 35ebbc5..7c62d3c 100644
--- a/third_party/ntcore_2018/src/main/native/cpp/ntcore_c.cpp
+++ b/third_party/ntcore_2018/src/main/native/cpp/ntcore_c.cpp
@@ -761,7 +761,7 @@
   str->len = 0;
 }
 
-void NT_DisposeEntryArray(NT_Entry* arr, size_t count) { std::free(arr); }
+void NT_DisposeEntryArray(NT_Entry* arr, size_t /*count*/) { std::free(arr); }
 
 void NT_DisposeConnectionInfoArray(NT_ConnectionInfo* arr, size_t count) {
   for (size_t i = 0; i < count; i++) DisposeConnectionInfo(&arr[i]);
diff --git a/third_party/wpiutil_2018/BUILD b/third_party/wpiutil_2018/BUILD
new file mode 100644
index 0000000..41822f6
--- /dev/null
+++ b/third_party/wpiutil_2018/BUILD
@@ -0,0 +1,27 @@
+licenses(['notice'])
+
+cc_library(
+  name = 'wpiutil',
+  visibility = ['//visibility:public'],
+  srcs = glob([
+    'src/main/native/cpp/**/*.cpp',
+    'src/main/native/cpp/**/*.h',
+    'src/main/native/cpp/**/*.inc',
+  ]),
+  copts = [
+    #'-Wno-switch-enum',
+    '-Wno-cast-align',
+    #'-Ithird_party/wpiutil_2018/
+    '-Wno-unused-parameter',
+  ],
+  hdrs = glob([
+    'src/main/native/include/**/*',
+  ]),
+  includes = [
+    'src/main/native/include',
+  ],
+  linkopts = [
+    '-lpthread',
+  ],
+  restricted_to = ['//tools:roborio'],
+)
diff --git a/tools/cpp/CROSSTOOL b/tools/cpp/CROSSTOOL
index 196912c..d4e7fbb 100644
--- a/tools/cpp/CROSSTOOL
+++ b/tools/cpp/CROSSTOOL
@@ -321,22 +321,22 @@
   compiler_flag: "--sysroot=external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi"
   compiler_flag: "-nostdinc"
   compiler_flag: "-isystem"
-  compiler_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/4.9.3/include",
+  compiler_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/5.4.0/include",
   compiler_flag: "-isystem"
-  compiler_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/4.9.3/include-fixed"
+  compiler_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/5.4.0/include-fixed"
   compiler_flag: "-isystem"
   compiler_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/usr/include"
 
   cxx_flag: "-isystem"
-  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include/c++/4.9.3"
+  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include/c++/5.4.0"
   cxx_flag: "-isystem"
-  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include/c++/4.9.3/arm-frc-linux-gnueabi"
+  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include/c++/5.4.0/arm-frc-linux-gnueabi"
   cxx_flag: "-isystem"
-  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include/c++/4.9.3/backward"
+  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include/c++/5.4.0/backward"
   cxx_flag: "-isystem"
-  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/4.9.3/include"
+  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/5.4.0/include"
   cxx_flag: "-isystem"
-  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/4.9.3/include-fixed"
+  cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/5.4.0/include-fixed"
   cxx_flag: "-isystem"
   cxx_flag: "external/arm_frc_linux_gnueabi_repo/usr/arm-frc-linux-gnueabi/include"
   cxx_flag: "-isystem"
@@ -346,11 +346,11 @@
   # used by gcc. That works because bazel currently doesn't track files at
   # absolute locations and has no remote execution, yet. However, this will need
   # to be fixed, maybe with auto-detection?
-  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%/c++/4.9.3"
-  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%/c++/4.9.3/arm-frc-linux-gnueabi"
-  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%/c++/4.9.3/backward"
-  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/4.9.3/include)%"
-  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/4.9.3/include-fixed)%"
+  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%/c++/5.4.0"
+  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%/c++/5.4.0/arm-frc-linux-gnueabi"
+  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%/c++/5.4.0/backward"
+  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/5.4.0/include)%"
+  cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/lib/x86_64-linux-gnu/gcc/arm-frc-linux-gnueabi/5.4.0/include-fixed)%"
   cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/include)%"
   cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/usr/include)%"
 
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index baefca6..59e715d 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -91,12 +91,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ =
-#ifdef WPILIB2015
-        DriverStation::GetInstance();
-#else
-        &DriverStation::GetInstance();
-#endif
 
     ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                         ::std::chrono::milliseconds(4));
@@ -114,7 +108,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     {
       auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -137,7 +131,6 @@
 
  private:
   int32_t my_pid_;
-  DriverStation *ds_;
 
   ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
   ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index a130544..d5f7e5e 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -252,12 +252,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ =
-#ifdef WPILIB2015
-        DriverStation::GetInstance();
-#else
-        &DriverStation::GetInstance();
-#endif
 
     top_reader_.Start();
     bottom_reader_.Start();
@@ -282,7 +276,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     const auto &values = constants::GetValues();
 
@@ -441,7 +435,6 @@
   }
 
   int32_t my_pid_;
-  DriverStation *ds_;
 
   ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
 
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index 83ec9a9..5165ce9 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -91,12 +91,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ =
-#ifdef WPILIB2015
-        DriverStation::GetInstance();
-#else
-        &DriverStation::GetInstance();
-#endif
 
     ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
                                         ::std::chrono::milliseconds(4));
@@ -114,7 +108,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     // Drivetrain
     {
@@ -142,7 +136,6 @@
 
  private:
   int32_t my_pid_;
-  DriverStation *ds_;
 
   ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
   ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 5e98b52..9bf2b0e 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -274,12 +274,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ =
-#ifdef WPILIB2015
-        DriverStation::GetInstance();
-#else
-        &DriverStation::GetInstance();
-#endif
 
     dma_synchronizer_->Start();
 
@@ -299,7 +293,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     const auto &values = constants::GetValues();
 
@@ -395,7 +389,6 @@
   }
 
   int32_t my_pid_;
-  DriverStation *ds_;
 
   ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
 
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 158f7a1..08ce044 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -298,7 +298,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ = &DriverStation::GetInstance();
 
     dma_synchronizer_->Start();
 
@@ -342,7 +341,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     const auto values = constants::GetValues();
 
@@ -472,7 +471,6 @@
   }
 
   int32_t my_pid_;
-  DriverStation *ds_;
 
   // Mutex to manage access to the period and tick time variables.
   ::aos::stl_mutex tick_time_mutex_;
diff --git a/y2017_bot3/wpilib_interface.cc b/y2017_bot3/wpilib_interface.cc
index 8fd2488..f9457ce 100644
--- a/y2017_bot3/wpilib_interface.cc
+++ b/y2017_bot3/wpilib_interface.cc
@@ -238,7 +238,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ = &DriverStation::GetInstance();
 
     dma_synchronizer_->Start();
 
@@ -282,7 +281,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     {
       auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -321,7 +320,6 @@
   }
 
   int32_t my_pid_;
-  DriverStation *ds_;
 
   // Mutex to manage access to the period and tick time variables.
   ::aos::stl_mutex tick_time_mutex_;
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 292fef9..4ca6ef7 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -353,7 +353,6 @@
     ::aos::SetCurrentThreadName("SensorReader");
 
     my_pid_ = getpid();
-    ds_ = &DriverStation::GetInstance();
 
     dma_synchronizer_->Start();
 
@@ -397,7 +396,7 @@
   }
 
   void RunIteration() {
-    ::frc971::wpilib::SendRobotState(my_pid_, ds_);
+    ::frc971::wpilib::SendRobotState(my_pid_);
 
     const auto values = constants::GetValues();
 
@@ -506,7 +505,6 @@
   }
 
   int32_t my_pid_;
-  DriverStation *ds_;
 
   // Mutex to manage access to the period and tick time variables.
   ::aos::stl_mutex tick_time_mutex_;