diff --git a/aos/vision/blob/stream_view.h b/aos/vision/blob/stream_view.h
index 9d63e8d..bee1dc9 100644
--- a/aos/vision/blob/stream_view.h
+++ b/aos/vision/blob/stream_view.h
@@ -10,30 +10,32 @@
 namespace aos {
 namespace vision {
 
-class BlobStreamViewer {
+class BlobStreamViewer : public DebugViewer {
  public:
-  BlobStreamViewer() : view_(false) {}
+  BlobStreamViewer() : DebugViewer(false) {}
+  explicit BlobStreamViewer(bool flip) : DebugViewer(flip) {}
+
   void Submit(ImageFormat fmt, const BlobList &blob_list) {
     SetFormatAndClear(fmt);
     DrawBlobList(blob_list, {255, 255, 255});
   }
 
   inline void SetFormatAndClear(ImageFormat fmt) {
-    if (fmt.w != ptr_.fmt().w || fmt.h != ptr_.fmt().h) {
+    if (!image_.fmt().Equals(fmt)) {
       printf("resizing data: %d, %d\n", fmt.w, fmt.h);
-      outbuf_.reset(new PixelRef[fmt.ImgSize()]);
-      ptr_ = ImagePtr{fmt, outbuf_.get()};
-      view_.UpdateImage(ptr_);
+      image_ = ImageValue(fmt);
+      UpdateImage(image_.get());
     }
-    memset(ptr_.data(), 0, fmt.ImgSize() * sizeof(PixelRef));
+    memset(image_.data(), 0, fmt.ImgSize() * sizeof(PixelRef));
   }
 
   inline void DrawBlobList(const BlobList &blob_list, PixelRef color) {
-    for (const auto &blob : blob_list) {
-      for (int i = 0; i < (int)blob.ranges.size(); ++i) {
-        for (const auto &range : blob.ranges[i]) {
+    ImagePtr ptr = img();
+    for (const RangeImage &blob : blob_list) {
+      for (int i = 0; i < (int)blob.ranges().size(); ++i) {
+        for (const auto &range : blob.ranges()[i]) {
           for (int j = range.st; j < range.ed; ++j) {
-            ptr_.get_px(j, i + blob.min_y) = color;
+            ptr.get_px(j, i + blob.min_y()) = color;
           }
         }
       }
@@ -42,15 +44,16 @@
 
   inline void DrawSecondBlobList(const BlobList &blob_list, PixelRef color1,
                                  PixelRef color2) {
+    ImagePtr ptr = img();
     for (const auto &blob : blob_list) {
-      for (int i = 0; i < (int)blob.ranges.size(); ++i) {
-        for (const auto &range : blob.ranges[i]) {
+      for (int i = 0; i < (int)blob.ranges().size(); ++i) {
+        for (const auto &range : blob.ranges()[i]) {
           for (int j = range.st; j < range.ed; ++j) {
-            auto px = ptr_.get_px(j, i + blob.min_y);
+            auto px = ptr.get_px(j, i + blob.min_y());
             if (px.r == 0 && px.g == 0 && px.b == 0) {
-              ptr_.get_px(j, i + blob.min_y) = color1;
+              ptr.get_px(j, i + blob.min_y()) = color1;
             } else {
-              ptr_.get_px(j, i + blob.min_y) = color2;
+              ptr.get_px(j, i + blob.min_y()) = color2;
             }
           }
         }
@@ -58,13 +61,13 @@
     }
   }
 
-  DebugViewer *view() { return &view_; }
+  // Backwards compatible.
+  DebugViewer *view() { return this; }
+
+  ImagePtr img() { return image_.get(); }
 
  private:
-  std::unique_ptr<PixelRef[]> outbuf_;
-  ImagePtr ptr_;
-
-  DebugViewer view_;
+  ImageValue image_;
 };
 
 }  // namespace vision
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index e003dc9..2b5b9e7 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -1,3 +1,4 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
 package(default_visibility = ["//visibility:public"])
 
 cc_library(
@@ -56,3 +57,12 @@
     '//aos/testing:googletest',
   ],
 )
+
+gtk_dependent_cc_library(
+  name = "gtk_event",
+  srcs = ["gtk_event.cc"],
+  deps = [
+    ":epoll_events",
+    '@usr_repo//:gtk+-3.0',
+  ],
+)
diff --git a/aos/vision/events/epoll_events.cc b/aos/vision/events/epoll_events.cc
index f191259..e4f789f 100644
--- a/aos/vision/events/epoll_events.cc
+++ b/aos/vision/events/epoll_events.cc
@@ -42,17 +42,10 @@
       }
       event->ReadEvent();
     }
-
-    for (EpollWatcher *watcher : watchers_) {
-      watcher->Wake();
-    }
   }
 }
 
 void EpollLoop::AddWait(EpollWait *wait) { waits_.push_back(wait); }
-void EpollLoop::AddWatcher(EpollWatcher *watcher) {
-  watchers_.push_back(watcher);
-}
 
 // Calculates the new timeout value to pass to epoll_wait.
 int EpollLoop::CalculateTimeout() {
diff --git a/aos/vision/events/epoll_events.h b/aos/vision/events/epoll_events.h
index d7574f9..ee288c4 100644
--- a/aos/vision/events/epoll_events.h
+++ b/aos/vision/events/epoll_events.h
@@ -17,6 +17,7 @@
 // Performs an asychronous wait using an EpollLoop.
 //
 // Note: this does not have very high resolution (sub-millisecond).
+// TODO(parker): This is mostly broken.
 class EpollWait {
  public:
   virtual ~EpollWait() {}
@@ -37,10 +38,15 @@
   int Recalculate(const monotonic_clock::time_point now) {
     if (time_ < monotonic_clock::epoch()) return -1;
     if (time_ <= now) {
-      Done();
       time_ = monotonic_clock::time_point(::std::chrono::seconds(-1));
-      return -1;
+      Done();
     }
+    // Duplicate above to allow Done to change itself.
+    if (time_ < monotonic_clock::epoch()) return -1;
+    if (time_ <= now) {
+      return -1;// Recalculate(now);
+    }
+
     if (time_ - now > ::std::chrono::milliseconds(INT_MAX)) {
       return INT_MAX;
     } else {
@@ -75,17 +81,6 @@
   EpollLoop *loop_ = nullptr;
 };
 
-// Provides a way for code to be notified every time after events are handled by
-// an EpollLoop. This is mainly a hack for the GTK integration and testing;
-// think very carefully before using it anywhere else.
-class EpollWatcher {
- public:
-  virtual ~EpollWatcher() {}
-
-  // Called after events have been processed each time the event loop wakes up.
-  virtual void Wake() = 0;
-};
-
 // A file descriptor based event loop implemented with epoll.
 class EpollLoop {
  public:
@@ -95,7 +90,6 @@
   // None of these take ownership of the passed-in objects.
   void AddWait(EpollWait *wait);
   void Add(EpollEvent *event);
-  void AddWatcher(EpollWatcher *watcher);
 
   // Delete event. Note that there are caveats here as this is
   // not idiot proof.
@@ -111,6 +105,10 @@
   // Loops forever, handling events.
   void Run();
 
+  // Fuses with gtk_main().
+  // Note that the dep for this is separate: //aos/vision/events:gtk_event
+  void RunWithGtkMain();
+
  private:
   int epoll_fd() { return epoll_fd_.get(); }
 
@@ -118,7 +116,6 @@
 
   ::aos::ScopedFD epoll_fd_;
   ::std::vector<EpollWait *> waits_;
-  ::std::vector<EpollWatcher *> watchers_;
 };
 
 }  // namespace events
diff --git a/aos/vision/events/gtk_event.cc b/aos/vision/events/gtk_event.cc
new file mode 100644
index 0000000..e8b43aa
--- /dev/null
+++ b/aos/vision/events/gtk_event.cc
@@ -0,0 +1,68 @@
+#include <gtk/gtk.h>
+#include <gdk/gdk.h>
+#include <thread>
+#include <sys/epoll.h>
+#include <mutex>
+#include <condition_variable>
+
+#include "aos/vision/events/epoll_events.h"
+
+namespace aos {
+namespace events {
+
+void EpollLoop::RunWithGtkMain() {
+  int timeout;
+  static constexpr size_t kNumberOfEvents = 64;
+  epoll_event events[kNumberOfEvents];
+  int number_events = 0;
+
+  std::mutex m;
+  std::condition_variable cv;
+  bool all_events_handled = false;
+  auto handle_cb = [&]() {
+    {
+      std::unique_lock<std::mutex> lk(m);
+
+      for (int i = 0; i < number_events; i++) {
+        EpollEvent *event = static_cast<EpollEvent *>(events[i].data.ptr);
+        if ((events[i].events & ~(EPOLLIN | EPOLLPRI)) != 0) {
+          LOG(FATAL, "unexpected epoll events set in %x on %d\n",
+              events[i].events, event->fd());
+        }
+        event->ReadEvent();
+      }
+      timeout = CalculateTimeout();
+
+      all_events_handled = true;
+    }
+    cv.notify_one();
+  };
+  handle_cb();
+  using HandleCBType = decltype(handle_cb);
+
+  std::thread t([&]() {
+    std::unique_lock<std::mutex> lk(m);
+    while (true) {
+      cv.wait(lk, [&all_events_handled]{return all_events_handled;});
+      // Wait for handle_cb to be done.
+      number_events = PCHECK(epoll_wait(epoll_fd(), events, kNumberOfEvents, timeout));
+      all_events_handled = false;
+      // Trigger handle_cb on main_thread to avoid concurrency.
+      gdk_threads_add_idle(+[](gpointer user_data) -> gboolean {
+        auto& handle_cb = *reinterpret_cast<HandleCBType*>(user_data);
+        handle_cb();
+        return G_SOURCE_REMOVE;
+      }, &handle_cb);
+    }
+  });
+  gtk_main();
+
+  // TODO(parker): Allow concurrent proxy onto the normal thread just like Gtk
+  // in order to allow event loop fusion, and make event addition thread-safe.
+
+  // Avoid stack destructors (explicitly shutting down of the thread.)
+  exit(EXIT_SUCCESS);
+}
+
+}  // namespace events
+}  // namespace aos
diff --git a/aos/vision/events/tcp_client.h b/aos/vision/events/tcp_client.h
index e7b80a6..7ce01f7 100644
--- a/aos/vision/events/tcp_client.h
+++ b/aos/vision/events/tcp_client.h
@@ -1,5 +1,5 @@
-#ifndef _AOS_VISION_DEBUG_TCP_SERVER_H_
-#define _AOS_VISION_DEBUG_TCP_SERVER_H_
+#ifndef _AOS_VISION_DEBUG_TCP_CLIENT_H_
+#define _AOS_VISION_DEBUG_TCP_CLIENT_H_
 
 #include "aos/vision/events/epoll_events.h"
 
@@ -19,4 +19,4 @@
 }  // namespace events
 }  // namespace aos
 
-#endif  // _AOS_VISION_DEBUG_TCP_SERVER_H_
+#endif  // _AOS_VISION_DEBUG_TCP_CLIENT_H_
diff --git a/aos/vision/image/jpeg_routines.cc b/aos/vision/image/jpeg_routines.cc
index e0c93dc..b6be41b 100644
--- a/aos/vision/image/jpeg_routines.cc
+++ b/aos/vision/image/jpeg_routines.cc
@@ -224,7 +224,7 @@
   int step = cinfo.num_components * cinfo.image_width;
   unsigned char *buffers[cinfo.image_height];
   for (size_t i = 0; i < cinfo.image_height; ++i) {
-    buffers[i] = reinterpret_cast<unsigned char *>(&out[offset]);
+    buffers[i] = &reinterpret_cast<unsigned char *>(out)[offset];
     offset += step;
   }
 
diff --git a/aos/vision/tools/BUILD b/aos/vision/tools/BUILD
new file mode 100644
index 0000000..56bbb5b
--- /dev/null
+++ b/aos/vision/tools/BUILD
@@ -0,0 +1,19 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+
+gtk_dependent_cc_binary(name = "jpeg_vision_test",
+  srcs = ["jpeg_vision_test.cc"],
+  deps = [
+    "//aos/common/logging:logging",
+    "//aos/common/logging:implementations",
+    "//aos/vision/math:vector",
+    "//aos/vision/image:reader",
+    "//aos/vision/image:jpeg_routines",
+    "//aos/vision/blob:threshold",
+    "//aos/vision/blob:range_image",
+    "//aos/vision/events:epoll_events",
+    "//aos/vision/blob:stream_view",
+    "//aos/vision/image:image_stream",
+    "//aos/vision/events:tcp_server",
+    "//aos/vision/events:gtk_event",
+  ],
+)
diff --git a/aos/vision/tools/jpeg_vision_test.cc b/aos/vision/tools/jpeg_vision_test.cc
new file mode 100644
index 0000000..37d9ffe
--- /dev/null
+++ b/aos/vision/tools/jpeg_vision_test.cc
@@ -0,0 +1,100 @@
+// This file is to collect data from a camera to use for debug and testing. This
+// should not placed on a robot. This is okay as it is a utility of limited use
+// only.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <netdb.h>
+#include <poll.h>
+#include <string.h>
+#include <gtk/gtk.h>
+#include <vector>
+#include <memory>
+#include <fstream>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/implementations.h"
+#include "aos/vision/math/vector.h"
+#include "aos/vision/image/reader.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/events/tcp_server.h"
+
+namespace aos {
+namespace vision {
+
+// Connects up a camera with our processing.
+class ChannelImageStream : public ImageStreamEvent {
+ public:
+  ChannelImageStream(const std::string &fname,
+                     const camera::CameraParams &params)
+      : ImageStreamEvent(fname, params), view_(true) {
+    // Lambda to record image data to a file on key press.
+    view_.view()->key_press_event = [this](uint32_t /*keyval*/) {
+      std::ofstream ofs("/tmp/test.jpg", std::ofstream::out);
+      ofs << prev_data_;
+      ofs.close();
+    };
+  }
+
+  // Handle an image from the camera.
+  void ProcessImage(DataRef data,
+                    aos::monotonic_clock::time_point /*timestamp*/) {
+    ImageFormat fmt = GetFmt(data);
+    if (!fmt.Equals(view_.img().fmt())) view_.SetFormatAndClear(fmt);
+    if (!ProcessJpeg(data, view_.img().data())) return;
+
+    ImagePtr img_ptr = view_.img();
+    prev_data_ = data.to_string();
+
+
+    // Threshold the image with the given lambda.
+    RangeImage rimg = DoThreshold(img_ptr, [](PixelRef &px) {
+      if (px.g > 88) {
+        uint8_t min = std::min(px.b, px.r);
+        uint8_t max = std::max(px.b, px.r);
+        if (min >= px.g || max >= px.g) return false;
+        uint8_t a = px.g - min;
+        uint8_t b = px.g - max;
+        return (a > 10 && b > 10);
+      }
+      return false;
+    });
+
+    view_.DrawBlobList({rimg}, {255, 255, 255});
+
+    view_.Redraw();
+  }
+
+ private:
+  std::string prev_data_;
+
+  // responsible for handling drawing
+  BlobStreamViewer view_;
+};
+}  // namespace aos
+}  // namespace vision
+
+int main(int argc, char *argv[]) {
+  ::aos::logging::Init();
+  ::aos::logging::AddImplementation(
+      new ::aos::logging::StreamLogImplementation(stdout));
+  aos::events::EpollLoop loop;
+  gtk_init(&argc, &argv);
+
+  camera::CameraParams params = {.width = 640 * 2,
+                                 .height = 480 * 2,
+                                 .exposure = 10,
+                                 .brightness = 128,
+                                 .gain = 0,
+                                 .fps = 10};
+
+  aos::vision::ChannelImageStream strm1("/dev/video1", params);
+
+  loop.Add(&strm1);
+  loop.RunWithGtkMain();
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7bbba74..6a2822b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -20,6 +20,7 @@
 #include "frc971/wpilib/imu.q.h"
 
 using frc971::sensors::gyro_reading;
+using frc971::imu_values;
 
 namespace frc971 {
 namespace control_loops {
@@ -121,8 +122,8 @@
     gear_logging.controller_index = kf_.controller_index();
     LOG_STRUCT(DEBUG, "state", gear_logging);
   }
-
-  if (::frc971::imu_values.FetchLatest()) {
+  const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
+  if (is_latest_imu_values) {
     const double rate = -::frc971::imu_values->gyro_y;
     const double accel_squared = ::frc971::imu_values->accelerometer_x *
                                      ::frc971::imu_values->accelerometer_x +
@@ -154,10 +155,40 @@
 
   // TODO(austin): Signal the current gear to both loops.
 
-  if (gyro_reading.FetchLatest()) {
-    LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
-    last_gyro_heading_ = gyro_reading->angle;
-    last_gyro_rate_ = gyro_reading->velocity;
+  switch (dt_config_.gyro_type) {
+    case GyroType::IMU_X_GYRO:
+      if (is_latest_imu_values) {
+        LOG_STRUCT(DEBUG, "using", *imu_values.get());
+        last_gyro_rate_ = imu_values->gyro_x;
+      }
+      break;
+    case GyroType::IMU_Y_GYRO:
+      if (is_latest_imu_values) {
+        LOG_STRUCT(DEBUG, "using", *imu_values.get());
+        last_gyro_rate_ = imu_values->gyro_y;
+      }
+      break;
+    case GyroType::IMU_Z_GYRO:
+      if (is_latest_imu_values) {
+        LOG_STRUCT(DEBUG, "using", *imu_values.get());
+        last_gyro_rate_ = imu_values->gyro_z;
+      }
+      break;
+    case GyroType::SPARTAN_GYRO:
+      if (gyro_reading.FetchLatest()) {
+        LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+        last_gyro_rate_ = gyro_reading->velocity;
+      }
+      break;
+    case GyroType::FLIPPED_SPARTAN_GYRO:
+      if (gyro_reading.FetchLatest()) {
+        LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+        last_gyro_rate_ = -gyro_reading->velocity;
+      }
+      break;
+    default:
+      LOG(FATAL, "invalid gyro configured");
+      break;
   }
 
   {
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 1330708..3483b60 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -39,7 +39,6 @@
 
   void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
 
-  double last_gyro_heading_ = 0.0;
   double last_gyro_rate_ = 0.0;
 
   const DrivetrainConfig dt_config_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 1fd633a..c0c7482 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -21,6 +21,14 @@
   CLOSED_LOOP = 1,  // Add in closed loop calculation.
 };
 
+enum class GyroType : int32_t {
+  SPARTAN_GYRO = 0, // Use the gyro on the spartan board.
+  IMU_X_GYRO = 1,   // Use the x-axis of the gyro on the IMU.
+  IMU_Y_GYRO = 2,   // Use the y-axis of the gyro on the IMU.
+  IMU_Z_GYRO = 3,   // Use the z-axis of the gyro on the IMU.
+  FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
+};
+
 struct DrivetrainConfig {
   // Shifting method we are using.
   ShifterType shifter_type;
@@ -28,6 +36,9 @@
   // Type of loop to use.
   LoopType loop_type;
 
+  // Type of gyro to use.
+  GyroType gyro_type;
+
   // Polydrivetrain functions returning various controller loops with plants.
   ::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
   ::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 21a968a..403d32c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -38,6 +38,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.cc b/y2012/control_loops/drivetrain/drivetrain_base.cc
index 321962e..8748d4d 100644
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2012::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2012::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.cc b/y2014/control_loops/drivetrain/drivetrain_base.cc
index 4df1a0f..ffb0ee3 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_base.cc
@@ -17,6 +17,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2014::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2014::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
index 1ebc4e9..afb5375 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2014_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2014_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2015/control_loops/drivetrain/drivetrain_base.cc b/y2015/control_loops/drivetrain/drivetrain_base.cc
index aae8674..26a283b 100644
--- a/y2015/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2015/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2015::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2015::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
index 3fe392a..f7c3d2f 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2015_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2015_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 1674540..f66d963 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -22,6 +22,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index 59139e1..e8d0b17 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -1,5 +1,6 @@
 load('/tools/build_rules/protobuf', 'proto_cc_library')
 load('/aos/build/queues', 'queue_library')
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
 
 queue_library(
   name = 'vision_queue',
@@ -107,6 +108,7 @@
       "//aos/vision/blob:hierarchical_contour_merge",
       "//aos/vision/blob:codec",
         ],
+  visibility = ['//visibility:public'],
 )
 
 cc_binary(
@@ -129,3 +131,25 @@
     '//aos/common:mutex',
   ],
 )
+
+gtk_dependent_cc_binary(
+  name = "debug_receiver",
+  srcs = ["debug_receiver.cc"],
+  visibility = ['//visibility:public'],
+  deps = [
+    "//aos/vision/image:image_types",
+    "//aos/vision/image:jpeg_routines",
+    "//aos/vision/events:socket_types",
+    "//aos/vision/events:tcp_client",
+    "//aos/vision/events:epoll_events",
+    "//aos/vision/events:gtk_event",
+    "//aos/vision/debug:debug_viewer",
+    "//aos/vision/blob:range_image",
+    "//aos/vision/blob:codec",
+    "//aos/vision/blob:stream_view",
+    ":stereo_geometry",
+    ":blob_filters",
+    ":vision_data",
+    ":calibration",
+  ],
+)
diff --git a/y2016/vision/debug_receiver.cc b/y2016/vision/debug_receiver.cc
new file mode 100644
index 0000000..7039c53
--- /dev/null
+++ b/y2016/vision/debug_receiver.cc
@@ -0,0 +1,193 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <netdb.h>
+#include <unistd.h>
+
+#include <vector>
+#include <memory>
+
+#include <gtk/gtk.h>
+#include "aos/vision/image/image_types.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/tcp_client.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/stream_view.h"
+
+#include "y2016/vision/vision_data.pb.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/vision/blob_filters.h"
+
+using namespace aos::vision;
+
+class StereoViewer {
+ public:
+  StereoViewer(int width, int height)
+      : blob_filt_(ImageFormat(width, height), 40, 100, 250000) {
+    overlays_.push_back(&overlay_);
+    view_.view()->SetOverlays(&overlays_);
+
+    // Uncomment to enable blob_filt_ overlay.
+    // blob_filt_.EnableOverlay(&overlay_);
+    finder_.EnableOverlay(&overlay_);
+  }
+
+  virtual ~StereoViewer() {}
+
+  void SetBlob(int camera_index, BlobList &blobl) {
+    if (camera_index == 0) {
+      left_blobs.swap(blobl);
+      new_left = true;
+    } else {
+      right_blobs.swap(blobl);
+      new_right = true;
+    }
+  }
+
+  void Process(ImageFormat fmt) {
+    if (new_left && new_right) {
+      overlay_.Reset();
+      DrawCross(overlay_, Vector<2>(fmt.w / 2.0, fmt.h / 2.0), {0, 255, 0});
+
+      view_.SetFormatAndClear(fmt);
+      printf("right (%d) left (%d)\n", (int)left_blobs.size(),
+             (int)right_blobs.size());
+      std::vector<std::pair<Vector<2>, Vector<2>>> cornersA =
+          finder_.Find(blob_filt_.FilterBlobs(left_blobs));
+      std::vector<std::pair<Vector<2>, Vector<2>>> cornersB =
+          finder_.Find(blob_filt_.FilterBlobs(right_blobs));
+      view_.DrawBlobList(left_blobs, {255, 0, 0});
+      view_.DrawSecondBlobList(right_blobs, {0, 255, 0}, {0, 0, 255});
+      view_.view()->Redraw();
+      new_left = false;
+      new_right = false;
+    }
+  }
+
+ private:
+  void DrawCross(PixelLinesOverlay &overlay, Vector<2> center, PixelRef color) {
+    overlay.add_line(Vector<2>(center.x() - 50, center.y()),
+                     Vector<2>(center.x() + 50, center.y()), color);
+    overlay.add_line(Vector<2>(center.x(), center.y() - 50),
+                     Vector<2>(center.x(), center.y() + 50), color);
+  }
+
+  // where we darw for debugging
+  PixelLinesOverlay overlay_;
+
+  // container for viewer
+  std::vector<OverlayBase *> overlays_;
+  BlobStreamViewer view_;
+
+  bool new_left = false;
+  BlobList left_blobs;
+  bool new_right = false;
+  BlobList right_blobs;
+
+  // our blob processing object
+  HistogramBlobFilter blob_filt_;
+
+  // corner finder to align aiming
+  CornerFinder finder_;
+};
+
+class BufferedLengthDelimReader {
+ public:
+  union data_len {
+    uint32_t len;
+    char buf[4];
+  };
+  BufferedLengthDelimReader() {
+    num_read_ = 0;
+    img_read_ = -1;
+  }
+  template <typename Lamb>
+  void up(int fd, Lamb lam) {
+    ssize_t count;
+    if (img_read_ < 0) {
+      count = read(fd, &len_.buf[num_read_], sizeof(len_.buf) - num_read_);
+      if (count < 0) return;
+      num_read_ += count;
+      if (num_read_ < 4) return;
+      num_read_ = 0;
+      img_read_ = 0;
+      data_.clear();
+      if (len_.len > 200000) {
+        printf("bad size: %d\n", len_.len);
+        exit(-1);
+      }
+      data_.resize(len_.len);
+    } else {
+      count = read(fd, &data_[img_read_], len_.len - img_read_);
+      if (count < 0) return;
+      img_read_ += count;
+      if (img_read_ < (int)len_.len) return;
+      lam(DataRef{&data_[0], len_.len});
+      img_read_ = -1;
+    }
+  }
+ private:
+  data_len len_;
+  int num_read_;
+  std::vector<char> data_;
+  int img_read_;
+};
+
+class ProtoClient : public aos::events::TcpClient {
+ public:
+  ProtoClient(int camera_index, int width, int height, const char *hostname,
+              int portno, StereoViewer *stereo)
+      : aos::events::TcpClient(hostname, portno),
+        camera_index_(camera_index),
+        fmt_(width, height),
+        stereo_(stereo) {}
+
+  void ReadEvent() override {
+    read_.up(fd(), [&](DataRef data) {
+      BlobList blobl;
+      y2016::vision::VisionData target;
+      if (target.ParseFromArray(data.data(), data.size())) {
+        auto &raw = target.raw();
+        ParseBlobList(&blobl, raw.data());
+        stereo_->SetBlob(camera_index_, blobl);
+        stereo_->Process(fmt_);
+      }
+    });
+  }
+
+  int camera_index_;
+
+  ImageFormat fmt_;
+
+  BufferedLengthDelimReader read_;
+  std::unique_ptr<PixelRef[]> outbuf;
+  ImagePtr ptr;
+  StereoViewer *stereo_;
+
+ private:
+};
+
+int main(int argc, char *argv[]) {
+  aos::events::EpollLoop loop;
+  gtk_init(&argc, &argv);
+
+  y2016::vision::Calibration calib =
+      y2016::vision::StereoGeometry("competition").calibration();
+
+  StereoViewer stereo(calib.camera_image_width(), calib.camera_image_height());
+
+  ProtoClient client0(0, calib.camera_image_width(),
+                      calib.camera_image_height(),
+                      calib.jetson_ip_addr().data(), 8082, &stereo);
+  ProtoClient client1(1, calib.camera_image_width(),
+                      calib.camera_image_height(),
+                      calib.jetson_ip_addr().data(), 8083, &stereo);
+
+  loop.Add(&client0);
+  loop.Add(&client1);
+  loop.RunWithGtkMain();
+  return EXIT_SUCCESS;
+}
diff --git a/y2016/vision/tools/BUILD b/y2016/vision/tools/BUILD
new file mode 100644
index 0000000..a9e25ac
--- /dev/null
+++ b/y2016/vision/tools/BUILD
@@ -0,0 +1,17 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+
+gtk_dependent_cc_binary(name = "blob_stream_replay",
+  srcs = ["blob_stream_replay.cc"],
+  deps = [
+    "//aos/vision/image:reader",
+    "//aos/vision/image:jpeg_routines",
+    "//aos/vision/image:image_stream",
+    "//aos/vision/events:epoll_events",
+    "//aos/vision/events:gtk_event",
+    "//aos/vision/events:tcp_server",
+    "//aos/vision/debug:debug_viewer",
+    "//aos/vision/blob:range_image",
+    "//aos/vision/blob:stream_view",
+    "//y2016/vision:blob_filters",
+  ],
+)
diff --git a/y2016/vision/tools/blob_stream_replay.cc b/y2016/vision/tools/blob_stream_replay.cc
new file mode 100644
index 0000000..f972fae
--- /dev/null
+++ b/y2016/vision/tools/blob_stream_replay.cc
@@ -0,0 +1,607 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <vector>
+#include <memory>
+#include <endian.h>
+#include <fstream>
+#include <gdk/gdk.h>
+#include <gtk/gtk.h>
+
+#include "aos/vision/image/reader.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/events/tcp_server.h"
+#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/blob/stream_view.h"
+#include "y2016/vision/blob_filters.h"
+// #include "y2016/vision/process_targets.h"
+
+namespace y2016 {
+namespace vision {
+using namespace aos::vision;
+
+::aos::vision::Vector<2> CreateCenterFromTarget(double lx, double ly, double rx, double ry) {
+  return ::aos::vision::Vector<2>((lx + rx) / 2.0, (ly + ry) / 2.0);
+}
+
+double TargetWidth(double lx, double ly, double rx, double ry) {
+  double dx = lx - rx;
+  double dy = ly - ry;
+  return ::std::hypot(dx, dy);
+}
+
+void SelectTargets(std::vector<std::pair<Vector<2>, Vector<2>>>& left_target,
+                   std::vector<std::pair<Vector<2>, Vector<2>>>&right_target,
+                   ::aos::vision::Vector<2> *center_left,
+                   ::aos::vision::Vector<2> *center_right) {
+  // No good targets. Let the caller decide defaults.
+  if (right_target.size() == 0 || left_target.size() == 0) {
+    return;
+  }
+
+  // Only one option, we have to go with it.
+  if (right_target.size() == 1 && left_target.size() == 1) {
+    *center_left =
+        CreateCenterFromTarget(left_target[0].first.x(), left_target[0].first.y(),
+                               left_target[0].second.x(), left_target[0].second.y());
+    *center_right = CreateCenterFromTarget(
+        right_target[0].first.x(), right_target[0].first.y(), right_target[0].second.x(),
+        right_target[0].second.y());
+    return;
+  }
+
+  // Now we have to make a decision.
+  double min_angle = -1.0;
+  int left_index = 0;
+  // First pick the widest target from the left.
+  for (size_t i = 0; i < left_target.size(); i++) {
+    const double h = left_target[i].first.y() -
+                     left_target[i].second.y();
+    const double wid1 = TargetWidth(left_target[i].first.x(),
+                                    left_target[i].first.y(),
+                                    left_target[i].second.x(),
+                                    left_target[i].second.y());
+    const double angle = h / wid1;
+    if (min_angle == -1.0 || ::std::abs(angle) < ::std::abs(min_angle)) {
+      min_angle = angle;
+      left_index = i;
+    }
+  }
+  // Calculate the angle of the bottom edge for the left.
+  double h = left_target[left_index].first.y() -
+             left_target[left_index].second.y();
+
+  double good_ang = min_angle;
+  double min_ang_err = -1.0;
+  int right_index = -1;
+  // Now pick the bottom edge angle from the right that lines up best with the left.
+  for (size_t j = 0; j < right_target.size(); j++) {
+    double wid2 = TargetWidth(right_target[j].first.x(),
+                                right_target[j].first.y(),
+                                right_target[j].second.x(),
+                                right_target[j].second.y());
+    h = right_target[j].first.y() -
+        right_target[j].second.y();
+    double ang = h/ wid2;
+    double ang_err = ::std::abs(good_ang - ang);
+    if (min_ang_err == -1.0 || min_ang_err > ang_err) {
+      min_ang_err = ang_err;
+      right_index = j;
+    }
+  }
+
+  *center_left =
+      CreateCenterFromTarget(left_target[left_index].first.x(),
+                             left_target[left_index].first.y(),
+                             left_target[left_index].second.x(),
+                             left_target[left_index].second.y());
+  *center_right =
+      CreateCenterFromTarget(right_target[right_index].first.x(),
+                             right_target[right_index].first.y(),
+                             right_target[right_index].second.x(),
+                             right_target[right_index].second.y());
+}
+
+
+long GetFileSize(std::string filename) {
+  struct stat stat_buf;
+  int rc = stat(filename.c_str(), &stat_buf);
+  return rc == 0 ? stat_buf.st_size : -1;
+}
+
+class OutputFile {
+ public:
+  OutputFile(const std::string &fname) : ofs(fname, std::ofstream::out) {}
+
+  void Emit(const BlobList &blobl, int64_t timestamp) {
+    int tmp_size = CalculateSize(blobl) + sizeof(int32_t) + sizeof(uint64_t);
+    tmp_buf.resize(tmp_size, 0);
+    {
+      char *buf = Int64Codec::Write(&tmp_buf[0], tmp_size);
+      buf = Int64Codec::Write(buf, timestamp);
+      SerializeBlob(blobl, buf);
+    }
+    ofs.write(&tmp_buf[0], tmp_size);
+    printf("blob_size: %d\n", tmp_size);
+  }
+
+  std::vector<char> tmp_buf;
+
+  std::ofstream ofs;
+};
+
+class InputFile {
+ public:
+  InputFile(const std::string &fname)
+      : ifs_(fname, std::ifstream::in), len_(GetFileSize(fname)) {
+    if (len_ <= 0) {
+      printf("File (%s) not found. Size (%d)\n", fname.c_str(), (int)len_);
+      fflush(stdout);
+    }
+    assert(len_ > 0);
+    tmp_buf_.resize(len_, 0);
+    ifs_.read(&tmp_buf_[0], len_);
+    buf_ = &tmp_buf_[0];
+  }
+
+  bool ReadNext(BlobList *blob_list, uint64_t *timestamp) {
+    if (buf_ - &tmp_buf_[0] >= len_) return false;
+    if (prev_ != nullptr) prev_frames_.emplace_back(prev_);
+    prev_ = buf_;
+    buf_ += sizeof(uint32_t);
+    *timestamp = Int64Codec::Read(&buf_);
+//    auto* buf_tmp = buf_;
+    buf_ = ParseBlobList(blob_list, buf_);
+//    fprintf(stderr, "read frame: %lu, buf_size: %lu\n", *timestamp, buf_ - buf_tmp);
+    return true;
+  }
+
+  bool ReadPrev(BlobList *blob_list, uint64_t *timestamp) {
+    if (prev_frames_.empty()) return false;
+    buf_ = prev_frames_.back();
+    prev_frames_.pop_back();
+    buf_ += sizeof(uint32_t);
+    *timestamp = Int64Codec::Read(&buf_);
+    buf_ = ParseBlobList(blob_list, buf_);
+    prev_ = nullptr;
+    return true;
+  }
+
+ private:
+  std::vector<const char *> prev_frames_;
+  const char *buf_;
+  const char *prev_ = nullptr;
+  std::ifstream ifs_;
+
+  long len_;
+  std::vector<char> tmp_buf_;
+};
+
+class BlobStreamFrame {
+ public:
+  BlobList blob_list;
+  uint64_t timestamp;
+  void ReadNext(InputFile *fin) {
+    blob_list.clear();
+    if (!fin->ReadNext(&blob_list, &timestamp)) {
+      exit(0);
+      return;
+    }
+  }
+  bool ReadPrev(InputFile *fin) {
+    blob_list.clear();
+    return fin->ReadPrev(&blob_list, &timestamp);
+  }
+};
+
+const char *kHudText =
+    "commands:\n"
+    " SPACE - pause\n"
+    " c - continue to next target\n"
+    " s - single step while paused\n"
+    " k - pause on next target frame\n"
+    " u - change window scaling\n"
+    " a - single step backward\n"
+    " q - quit\n"
+    " h - help\n";
+
+class NetworkForwardingImageStream : public aos::events::EpollWait {
+ public:
+  NetworkForwardingImageStream(ImageFormat fmt, int debug_level,
+                               const std::string &fname1,
+                               const std::string &fname2)
+      : fmt_(fmt),
+        ifs1_(fname1),
+        ifs2_(fname2),
+        blob_filt_(fmt, 40, 750, 250000),
+        finder_(0.25, 35) {
+    text_overlay_.draw_fn =
+        [this](RenderInterface *render, double /*width*/, double /*height*/) {
+      render->SetSourceRGB(1.0, 1.0, 1.0);
+      if (hud_text) render->Text(20, 20, 0, 0, kHudText);
+    };
+
+    overlays_.push_back(&overlay_);
+    overlays_.push_back(&text_overlay_);
+    view_.view()->SetOverlays(&overlays_);
+
+    if (debug_level > 0) {
+      finder_.EnableOverlay(&overlay_);
+    }
+    if (debug_level > 1) {
+      blob_filt_.EnableOverlay(&overlay_);
+    }
+
+    frame1.ReadNext(&ifs1_);
+    frame2.ReadNext(&ifs2_);
+
+    std::pair<int, int> skip =
+        TickFrame(std::max(frame1.timestamp, frame2.timestamp));
+    printf("Initialzation skipped (%d, %d)\n", skip.first, skip.second);
+
+    ms_event_delta_ = 20;
+    play_forward = true;
+    paused = false;
+    single_step = false;
+    pause_on_next_target = true;
+    continue_to_next_target = false;
+    view_.view()->SetScale(scale_factor);
+    view_.view()->key_press_event = [this](uint32_t keyval) {
+      play_forward = true;
+      switch (keyval) {
+        case GDK_KEY_space:
+          paused = !paused;
+          pause_on_next_target = false;
+          continue_to_next_target = false;
+          break;
+        case GDK_KEY_c:
+          pause_on_next_target = true;
+          continue_to_next_target = true;
+          paused = false;
+          break;
+        case GDK_KEY_s:
+          single_step = true;
+          continue_to_next_target = false;
+          paused = true;
+          break;
+        case GDK_KEY_k:
+          pause_on_next_target = true;
+          continue_to_next_target = false;
+          paused = false;
+          break;
+        case GDK_KEY_u:
+          if (scale_factor == 1.0) {
+            scale_factor = 0.75;
+            view_.view()->SetScale(0.75);
+          } else {
+            scale_factor = 1.0;
+            view_.view()->SetScale(1.0);
+            view_.view()->MoveTo(150, -220);
+          }
+          break;
+        case GDK_KEY_a:
+          play_forward = false;
+          single_step = true;
+          paused = true;
+          break;
+        case GDK_KEY_q:
+          exit(0);
+        case GDK_KEY_h:
+          hud_text = !hud_text;
+          break;
+        default:
+          printf("pressed: %s\n", gdk_keyval_name(keyval));
+      }
+    };
+  }
+
+  double scale_factor = 1.0;
+  bool hud_text = true;
+  bool play_forward;
+  bool paused;
+  bool single_step;
+  bool pause_on_next_target;
+  bool continue_to_next_target;
+
+  std::string distance_text;
+
+  std::pair<int, int> TickFrame(uint64_t time) {
+    timestamp_ += time;
+    return TickToFrame(timestamp_);
+  }
+
+  std::pair<int, int> TickBackFrame(uint64_t time) {
+    timestamp_ -= time;
+    return TickBackToFrame(timestamp_);
+  }
+
+  std::pair<int, int> TickToFrame(uint64_t timestamp) {
+    std::pair<int, int> skip(0, 0);
+    while (frame1.timestamp < timestamp) {
+      frame1.ReadNext(&ifs1_);
+      skip.first++;
+    }
+    while (frame2.timestamp < timestamp) {
+      frame2.ReadNext(&ifs2_);
+      skip.second++;
+    }
+    return skip;
+  }
+
+  std::pair<int, int> TickBackToFrame(uint64_t timestamp) {
+    std::pair<int, int> skip(0, 0);
+    while (frame1.timestamp >= timestamp) {
+      if (!frame1.ReadPrev(&ifs1_)) break;
+      skip.first++;
+    }
+    while (frame2.timestamp >= timestamp) {
+      if (!frame2.ReadPrev(&ifs2_)) break;
+      skip.second++;
+    }
+    frame1.ReadPrev(&ifs1_);
+    frame2.ReadPrev(&ifs2_);
+    return skip;
+  }
+  BlobStreamFrame frame1;
+  BlobStreamFrame frame2;
+  uint64_t timestamp_ = 0;
+
+  Vector<2> GetCenter(const BlobList &blob_list) {
+    std::vector<std::pair<Vector<2>, Vector<2>>> corners =
+        finder_.Find(blob_filt_.FilterBlobs(blob_list));
+
+    if (corners.size() == 1) {
+      Vector<2> center = (corners[0].first + corners[0].second) * (0.5);
+      return center;
+    }
+    return {0, 0};
+  }
+
+  void DrawSuperSpeed() {
+    PixelRef color = {0, 255, 255};
+    // S
+    overlay_.add_line(Vector<2>(200, 100), Vector<2>(100, 100), color);
+    overlay_.add_line(Vector<2>(100, 100), Vector<2>(100, 300), color);
+    overlay_.add_line(Vector<2>(100, 300), Vector<2>(200, 300), color);
+    overlay_.add_line(Vector<2>(200, 300), Vector<2>(200, 500), color);
+    overlay_.add_line(Vector<2>(200, 500), Vector<2>(100, 500), color);
+    // U
+    overlay_.add_line(Vector<2>(250, 100), Vector<2>(250, 500), color);
+    overlay_.add_line(Vector<2>(250, 500), Vector<2>(350, 500), color);
+    overlay_.add_line(Vector<2>(350, 500), Vector<2>(350, 100), color);
+    // P
+    overlay_.add_line(Vector<2>(400, 100), Vector<2>(400, 500), color);
+    overlay_.add_line(Vector<2>(400, 100), Vector<2>(500, 100), color);
+    overlay_.add_line(Vector<2>(500, 100), Vector<2>(500, 300), color);
+    overlay_.add_line(Vector<2>(500, 300), Vector<2>(400, 300), color);
+    // E
+    overlay_.add_line(Vector<2>(550, 100), Vector<2>(550, 500), color);
+    overlay_.add_line(Vector<2>(550, 100), Vector<2>(650, 100), color);
+    overlay_.add_line(Vector<2>(550, 300), Vector<2>(650, 300), color);
+    overlay_.add_line(Vector<2>(550, 500), Vector<2>(650, 500), color);
+    // R
+    overlay_.add_line(Vector<2>(700, 100), Vector<2>(700, 500), color);
+    overlay_.add_line(Vector<2>(700, 100), Vector<2>(800, 100), color);
+    overlay_.add_line(Vector<2>(800, 100), Vector<2>(800, 300), color);
+    overlay_.add_line(Vector<2>(800, 300), Vector<2>(700, 300), color);
+    overlay_.add_line(Vector<2>(700, 350), Vector<2>(800, 500), color);
+    // S
+    overlay_.add_line(Vector<2>(200, 550), Vector<2>(100, 550), color);
+    overlay_.add_line(Vector<2>(100, 550), Vector<2>(100, 750), color);
+    overlay_.add_line(Vector<2>(100, 750), Vector<2>(200, 750), color);
+    overlay_.add_line(Vector<2>(200, 750), Vector<2>(200, 950), color);
+    overlay_.add_line(Vector<2>(200, 950), Vector<2>(100, 950), color);
+    // P
+    overlay_.add_line(Vector<2>(250, 550), Vector<2>(250, 950), color);
+    overlay_.add_line(Vector<2>(250, 550), Vector<2>(350, 550), color);
+    overlay_.add_line(Vector<2>(350, 550), Vector<2>(350, 750), color);
+    overlay_.add_line(Vector<2>(350, 750), Vector<2>(250, 750), color);
+    // E
+    overlay_.add_line(Vector<2>(400, 550), Vector<2>(400, 950), color);
+    overlay_.add_line(Vector<2>(400, 550), Vector<2>(500, 550), color);
+    overlay_.add_line(Vector<2>(400, 750), Vector<2>(500, 750), color);
+    overlay_.add_line(Vector<2>(400, 950), Vector<2>(500, 950), color);
+    // E
+    overlay_.add_line(Vector<2>(550, 550), Vector<2>(550, 950), color);
+    overlay_.add_line(Vector<2>(550, 550), Vector<2>(650, 550), color);
+    overlay_.add_line(Vector<2>(550, 750), Vector<2>(650, 750), color);
+    overlay_.add_line(Vector<2>(550, 950), Vector<2>(650, 950), color);
+    // D
+    overlay_.add_line(Vector<2>(700, 550), Vector<2>(700, 950), color);
+    overlay_.add_line(Vector<2>(700, 550), Vector<2>(800, 575), color);
+    overlay_.add_line(Vector<2>(800, 575), Vector<2>(800, 925), color);
+    overlay_.add_line(Vector<2>(800, 925), Vector<2>(700, 950), color);
+  }
+
+  void UpdateNewTime(int new_delta) {
+    if (new_delta != ms_event_delta_) {
+      ms_event_delta_ = new_delta;
+      SetTime(::std::chrono::milliseconds(ms_event_delta_) + aos::monotonic_clock::now());
+    }
+  }
+
+  void Done() override {
+    SetTime(::std::chrono::milliseconds(ms_event_delta_) + aos::monotonic_clock::now());
+    if (paused && !single_step) return;
+    single_step = false;
+    frame_count_++;
+
+    while (true) {
+      overlay_.Reset();
+      view_.SetFormatAndClear(fmt_);
+      std::pair<int, int> skipped(1, 1);
+      // how far we will step to look for the next target
+      int nano_step = 300 * 1e6;
+      if (play_forward && seeking_target_) {
+        skipped = TickFrame(nano_step);
+      } else if (seeking_target_) {
+        skipped = TickBackFrame(nano_step);
+      } else if (play_forward) {
+        frame1.ReadNext(&ifs1_);
+        frame2.ReadNext(&ifs2_);
+      } else {
+        frame1.ReadPrev(&ifs1_);
+        frame2.ReadPrev(&ifs2_);
+      }
+      // printf("skipped (%d, %d)\n", skipped.first, skipped.second);
+
+      std::vector<std::pair<Vector<2>, Vector<2>>> corner1 =
+          finder_.Find(blob_filt_.FilterBlobs(frame1.blob_list));
+      std::vector<std::pair<Vector<2>, Vector<2>>> corner2 =
+          finder_.Find(blob_filt_.FilterBlobs(frame2.blob_list));
+
+      Vector<2> cent1;
+      Vector<2> cent2;
+      SelectTargets(corner1, corner2, &cent1, &cent2);
+
+      /*
+      int target_count_;
+      if (cent1 == Vector<2>(0, 0) && cent2 == Vector<2>(0, 0)) {
+        missed_count_ += std::min(skipped.first, skipped.second);
+        if (missed_count_ > 15) {
+          seeking_target_ = true;
+          DrawSuperSpeed();
+          SetTime(aos::vision::MsTime(1));
+          if (line_break_) {
+            printf("_-_-_-%d_-_-_-_\n", target_count_);
+            target_count_++;
+            line_break_ = false;
+          }
+          if (continue_to_next_target) {
+            continue_to_next_target = false;
+          }
+          continue;
+        }
+      } else {
+        missed_count_ = 0;
+      }
+      */
+
+      if (seeking_target_) {
+        if (play_forward) {
+          // Go back to the last time we didn't see a target and play from there.
+          TickBackFrame(nano_step);
+          seeking_target_ = false;
+        } else if (seeking_target_) {
+          TickFrame(nano_step);
+          seeking_target_ = false;
+        }
+        continue;
+      }
+
+      // comment out to turn off full blob drawing
+      view_.DrawBlobList(frame1.blob_list, {0, 0, 255});
+      view_.DrawSecondBlobList(frame2.blob_list, {0, 255, 0}, {0, 255, 255});
+
+      DrawCross(overlay_, Vector<2>(fmt_.w / 2.0, fmt_.h / 2.0), {255, 0, 0});
+
+      double timeFromEpoch =
+          1e-9 * ((double)frame1.timestamp + (double)frame2.timestamp) / 2;
+      printf("timestamp: %g skew: %g\n", timeFromEpoch,
+             1e-9 * ((double)frame1.timestamp - (double)frame2.timestamp));
+      /*
+      if (cent1 == Vector<2>(0, 0) && cent2 == Vector<2>(0, 0)) {
+      } else {
+        DrawCross(overlay_, cent1, {255, 255, 255});
+        DrawCross(overlay_, cent2, {255, 255, 255});
+        double x = (cent1.x() + cent2.x()) / 2.0;
+        DrawCross(overlay_, Vector<2>(x, fmt_.h / 2.0), {255, 255, 255});
+        SetTime(aos::vision::MsTime(100));
+        if (pause_on_next_target && !continue_to_next_target) {
+          paused = true;
+          pause_on_next_target = false;
+        }
+        line_break_ = true;
+        missed_count_ = 0;
+      }
+      fflush(stdout);
+      */
+      view_.view()->Redraw();
+      break;
+    }
+  }
+
+  void DrawCross(PixelLinesOverlay &overlay, Vector<2> center, PixelRef color) {
+    overlay.add_line(Vector<2>(center.x() - 25, center.y()),
+                     Vector<2>(center.x() + 25, center.y()), color);
+    overlay.add_line(Vector<2>(center.x(), center.y() - 25),
+                     Vector<2>(center.x(), center.y() + 25), color);
+  }
+
+  void AddTo(aos::events::EpollLoop *loop) {
+    Done();
+    loop->AddWait(this);
+  }
+
+  std::unique_ptr<PixelRef[]> outbuf;
+  ImagePtr ptr;
+
+  BlobStreamViewer view_;
+
+ private:
+  int ms_event_delta_ = 200;
+ public:
+  // basic image size
+  ImageFormat fmt_;
+
+  // where we darw for debugging
+  PixelLinesOverlay overlay_;
+
+  // Where we draw text on the screen.
+  LambdaOverlay text_overlay_;
+  // container for viewer
+  std::vector<OverlayBase *> overlays_;
+
+  InputFile ifs1_;
+  InputFile ifs2_;
+
+  // our blob processing object
+  HistogramBlobFilter blob_filt_;
+
+  // corner finder to align aiming
+  CornerFinder finder_;
+
+  // indicates we have lost a target
+  bool line_break_ = false;
+
+  // indicates we are looking for the next target
+  bool seeking_target_ = false;
+
+  int frame_count_ = 0;
+
+  // count how many frames we miss in a row.
+  int missed_count_ = 16;
+};
+}
+}  // namespace y2016::vision
+
+int main(int argc, char *argv[]) {
+  using namespace y2016::vision;
+  aos::events::EpollLoop loop;
+  gtk_init(&argc, &argv);
+
+  if (argc != 3) {
+    printf("Wrong number of arguments. Got (%d) expected 3.\n", argc);
+    printf(
+        " arguments are debug level as {0, 1, 2} and then filename without the "
+        "{_0.dat,_1.dat} suffixes\n");
+  }
+
+  int dbg = std::stoi(argv[1]);
+
+  std::string file(argv[2]);
+  aos::vision::ImageFormat fmt = {640 * 2, 480 * 2};
+
+  printf("file (%s) dbg_lvl (%d)\n", file.c_str(), dbg);
+
+  std::string fname_path = file;
+  NetworkForwardingImageStream strm1(
+      fmt, dbg, fname_path + "_0.dat", fname_path + "_1.dat");
+  fprintf(stderr, "staring main\n");
+  strm1.AddTo(&loop);
+
+  fprintf(stderr, "staring main\n");
+  loop.RunWithGtkMain();
+}
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
index 5997342..26a551b 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2016_bot3::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2016_bot3::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
index 967e8df..54a7321 100644
--- a/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot4/control_loops/drivetrain/drivetrain_base.cc
@@ -21,6 +21,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
 
       ::y2016_bot4::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2016_bot4::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
index 0eb506e..1e30e08 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -22,6 +22,7 @@
   static DrivetrainConfig kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::IMU_Y_GYRO,
 
       ::y2017::control_loops::drivetrain::MakeDrivetrainLoop,
       ::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,