diff --git a/aos/network/www/config_handler.ts b/aos/network/www/config_handler.ts
index 72b496e..3d20f0d 100644
--- a/aos/network/www/config_handler.ts
+++ b/aos/network/www/config_handler.ts
@@ -1,18 +1,35 @@
 import {Configuration, Channel} from 'aos/configuration_generated';
 import {Connect} from 'aos/network/connect_generated';
+import {Connection} from './proxy';
 
 export class ConfigHandler {
   private readonly root_div = document.getElementById('config');
+  private readonly tree_div;
+  private config: Configuration|null = null
 
-  constructor(
-      private readonly config: Configuration,
-      private readonly dataChannel: RTCDataChannel) {}
+  constructor(private readonly connection: Connection) {
+    this.connection.addConfigHandler((config) => this.handleConfig(config));
+
+    const show_button = document.createElement('button');
+    show_button.addEventListener('click', () => this.toggleConfig());
+    const show_text = document.createTextNode('Show/Hide Config');
+    show_button.appendChild(show_text);
+    this.tree_div = document.createElement('div');
+    this.tree_div.hidden = true;
+    this.root_div.appendChild(show_button);
+    this.root_div.appendChild(this.tree_div);
+  }
+
+  handleConfig(config: Configuration) {
+    this.config = config;
+    this.printConfig();
+  }
 
   printConfig() {
     for (const i = 0; i < this.config.channelsLength(); i++) {
       const channel_div = document.createElement('div');
       channel_div.classList.add('channel');
-      this.root_div.appendChild(channel_div);
+      this.tree_div.appendChild(channel_div);
 
       const input_el = document.createElement('input');
       input_el.setAttribute('data-index', i);
@@ -60,8 +77,10 @@
     Connect.addChannelsToTransfer(builder, channelsfb);
     const connect = Connect.endConnect(builder);
     builder.finish(connect);
-    const array = builder.asUint8Array();
-    console.log('connect', array);
-    this.dataChannel.send(array.buffer.slice(array.byteOffset));
+    this.connection.sendConnectMessage(builder);
+  }
+
+  toggleConfig() {
+    this.tree_div.hidden = !this.tree_div.hidden;
   }
 }
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 704fc85..9eb0bb6 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -49,8 +49,11 @@
   private rtcPeerConnection: RTCPeerConnection|null = null;
   private dataChannel: DataChannel|null = null;
   private webSocketUrl: string;
-  private configHandler: ConfigHandler|null = null;
-  private config: Configuration|null = null;
+
+  private configInternal: Configuration|null = null;
+  // A set of functions that accept the config to handle.
+  private readonly configHandlers = new Set<(config: Configuration) => void>();
+
   private readonly handlerFuncs = new Map<string, (data: Uint8Array) => void>();
   private readonly handlers = new Set<Handler>();
 
@@ -59,6 +62,10 @@
     this.webSocketUrl = `ws://${server}/ws`;
   }
 
+  addConfigHandler(handler: (config: Configuration) => void): void {
+    this.configHandlers.add(handler);
+  }
+
   addHandler(id: string, handler: (data: Uint8Array) => void): void {
     this.handlerFuncs.set(id, handler);
   }
@@ -72,17 +79,17 @@
         'message', (e) => this.onWebSocketMessage(e));
   }
 
+  get config() {
+    return this.config_internal;
+  }
+
   // Handle messages on the DataChannel. Handles the Configuration message as
   // all other messages are sent on specific DataChannels.
   onDataChannelMessage(e: MessageEvent): void {
     const fbBuffer = new flatbuffers.ByteBuffer(new Uint8Array(e.data));
-    // TODO(alex): handle config updates if/when required
-    if (!this.configHandler) {
-      const config = Configuration.getRootAsConfiguration(fbBuffer);
-      this.config = config;
-      this.configHandler = new ConfigHandler(config, this.dataChannel);
-      this.configHandler.printConfig();
-      return;
+    this.configInternal = Configuration.getRootAsConfiguration(fbBuffer);
+    for (handler of this.configHandlers) {
+      handler(this.configInternal);
     }
   }
 
@@ -174,4 +181,13 @@
         break;
     }
   }
+
+  /**
+   * Subscribes to messages.
+   * @param a Finished flatbuffer.Builder containing a Connect message to send.
+   */
+  sendConnectMessage(builder: any) {
+    const array = builder.assUint8Array();
+    this.dataChannel.send(array.buffer.slice(array.byteOffset));
+  }
 }
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d769541..f033a89 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -142,7 +142,7 @@
     A_.setZero();
     B_.setZero();
     DelayedU_.setZero();
-    UpdateAB(::std::chrono::milliseconds(5));
+    UpdateAB(::std::chrono::microseconds(5050));
   }
 
   // Assert that U is within the hardware range.
@@ -158,6 +158,8 @@
   // Computes the new X and Y given the control input.
   void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
               ::std::chrono::nanoseconds dt, Scalar voltage_battery) {
+    CHECK_NE(dt, std::chrono::nanoseconds(0));
+
     // Powers outside of the range are more likely controller bugs than things
     // that the plant should deal with.
     CheckU(U);
@@ -178,7 +180,11 @@
     // or the unit delay...  Might want to do that if we care about performance
     // again.
     UpdateAB(dt);
-    return A() * X + B() * U;
+    const Eigen::Matrix<Scalar, number_of_states, 1> new_X =
+        A() * X + B() * DelayedU_;
+    DelayedU_ = U;
+
+    return new_X;
   }
 
  protected:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 27a659a..9c36c90 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -284,9 +284,9 @@
 
     def InitializeState(self):
         """Sets X, Y, and X_hat to zero defaults."""
-        self.X = numpy.zeros((self.A.shape[0], 1))
+        self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
         self.Y = self.C * self.X
-        self.X_hat = numpy.zeros((self.A.shape[0], 1))
+        self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
 
     def PlaceControllerPoles(self, poles):
         """Places the controller poles.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d63bca7..168df43 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -108,6 +108,8 @@
   }
   Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
 
+  const std::chrono::nanoseconds dt() const { return coefficients().dt; }
+
   const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
   Scalar X(int i, int j = 0) const { return X()(i, j); }
   const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
@@ -503,9 +505,7 @@
     return controller().Kff() * (next_R() - plant().A() * R());
   }
 
-  // stop_motors is whether or not to output all 0s.
-  void Update(bool stop_motors,
-              ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
+  void UpdateController(bool stop_motors) {
     if (stop_motors) {
       U_.setZero();
       U_uncapped_.setZero();
@@ -514,10 +514,15 @@
       U_ = U_uncapped_ = ControllerOutput();
       CapU();
     }
+    UpdateFFReference();
+  }
+
+  // stop_motors is whether or not to output all 0s.
+  void Update(bool stop_motors,
+              ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(0)) {
+    UpdateController(stop_motors);
 
     UpdateObserver(U_, dt);
-
-    UpdateFFReference();
   }
 
   // Updates R() after any CapU operations happen on U().
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index 5ebf417..42d4a7c 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -82,6 +82,8 @@
     ssh_path = "external/ssh/ssh"
     scp_path = "external/ssh/scp"
 
+    subprocess.check_call([ssh_path, ssh_target, "mkdir", "-p", target_dir])
+
     rsync_cmd = ([
         "external/rsync/usr/bin/rsync", "-e", ssh_path, "-c", "-v", "-z",
         "--copy-links"
diff --git a/y2020/BUILD b/y2020/BUILD
index f3ea4d6..4aa851f 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -166,6 +166,7 @@
     srcs = ["web_proxy.sh"],
     data = [
         ":config.json",
+        "//y2020/www:field_main_bundle",
         "//aos/network:web_proxy_main",
         "//y2020/www:files",
         "//y2020/www:flatbuffers",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index f0610a0..463919a 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -102,15 +102,15 @@
       break;
 
     case kCompTeamNumber:
-      hood->zeroing_constants.measured_absolute_position = 0.432541963515072;
+      hood->zeroing_constants.measured_absolute_position = 0.266691815725476;
 
       intake->zeroing_constants.measured_absolute_position =
           1.42977866919024 - Values::kIntakeZero();
 
-      turret->potentiometer_offset =
-          5.52519370141247 + 0.00853506822980376 + 0.0109413725126625;
+      turret->potentiometer_offset = 5.52519370141247 + 0.00853506822980376 +
+                                     0.0109413725126625 - 0.223719825811759;
       turret_params->zeroing_constants.measured_absolute_position =
-          0.251065633255048;
+          0.547478339799516;
       break;
 
     case kPracticeTeamNumber:
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 0add232..d26552a 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -23,6 +23,9 @@
   return result;
 }
 
+// Indices of the pis to use.
+const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+
 }  // namespace
 
 Localizer::Localizer(
@@ -51,9 +54,11 @@
                            ekf_.P());
   });
 
-  image_fetchers_.emplace_back(
-      event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
-          "/pi1/camera"));
+  for (const auto &pi : kPisToUse) {
+    image_fetchers_.emplace_back(
+        event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
+            "/" + pi + "/camera"));
+  }
 
   target_selector_.set_has_target(false);
 }
@@ -89,9 +94,10 @@
                        aos::monotonic_clock::time_point now,
                        double left_encoder, double right_encoder,
                        double gyro_rate, const Eigen::Vector3d &accel) {
-  for (auto &image_fetcher : image_fetchers_) {
+  for (size_t ii = 0; ii < kPisToUse.size(); ++ii) {
+    auto &image_fetcher = image_fetchers_[ii];
     while (image_fetcher.FetchNext()) {
-      HandleImageMatch(*image_fetcher);
+      HandleImageMatch(kPisToUse[ii], *image_fetcher);
     }
   }
   ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
@@ -99,13 +105,13 @@
 }
 
 void Localizer::HandleImageMatch(
-    const frc971::vision::sift::ImageMatchResult &result) {
+    std::string_view pi, const frc971::vision::sift::ImageMatchResult &result) {
   std::chrono::nanoseconds monotonic_offset(0);
   clock_offset_fetcher_.Fetch();
   if (clock_offset_fetcher_.get() != nullptr) {
     for (const auto connection : *clock_offset_fetcher_->connections()) {
       if (connection->has_node() && connection->node()->has_name() &&
-          connection->node()->name()->string_view() == "pi1") {
+          connection->node()->name()->string_view() == pi) {
         monotonic_offset =
             std::chrono::nanoseconds(connection->monotonic_offset());
         break;
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 8636a7a..1d6f816 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -1,6 +1,8 @@
 #ifndef Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
 #define Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
 
+#include <string_view>
+
 #include "aos/containers/ring_buffer.h"
 #include "aos/events/event_loop.h"
 #include "aos/network/message_bridge_server_generated.h"
@@ -69,8 +71,9 @@
     double velocity = 0.0;  // rad/sec
   };
 
-  // Processes new image data and updates the EKF.
-  void HandleImageMatch(const frc971::vision::sift::ImageMatchResult &result);
+  // Processes new image data from the given pi and updates the EKF.
+  void HandleImageMatch(std::string_view pi,
+                        const frc971::vision::sift::ImageMatchResult &result);
 
   // Processes the most recent turret position and stores it in the turret_data_
   // buffer.
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 1746589..48c9098 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -28,20 +28,22 @@
     name='Accelerator',
     motor=control_loop.Falcon(),
     G=G,
-    J=J,
-    q_pos=0.08,
-    q_vel=4.00,
-    q_voltage=0.4,
+    J=J + 0.0015,
+    q_pos=0.01,
+    q_vel=40.0,
+    q_voltage=2.0,
     r_pos=0.05,
-    controller_poles=[.84])
+    controller_poles=[.86])
 
 
 def main(argv):
     if FLAGS.plot:
         R = numpy.matrix([[0.0], [500.0], [0.0]])
-        flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
+        flywheel.PlotSpinup(kAccelerator, goal=R, iterations=400)
         return 0
 
+    glog.debug("J is %f" % J)
+
     if len(argv) != 5:
         glog.fatal('Expected .h file name and .cc file name')
     else:
@@ -53,4 +55,5 @@
 
 if __name__ == '__main__':
     argv = FLAGS(sys.argv)
+    glog.init()
     sys.exit(main(argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 5067a16..9381630 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -14,7 +14,7 @@
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
 # Inertia for a single 4" diameter, 2" wide neopreme wheel.
-J_wheel = 0.000319 * 2.0
+J_wheel = 0.000319 * 2.0 * 6.0
 # Gear ratio to the final wheel.
 # 40 tooth on the flywheel
 # 48 for the falcon.
@@ -29,17 +29,17 @@
     motor=control_loop.Falcon(),
     G=G,
     J=J,
-    q_pos=0.08,
-    q_vel=4.00,
-    q_voltage=0.4,
+    q_pos=0.01,
+    q_vel=100.0,
+    q_voltage=6.0,
     r_pos=0.05,
-    controller_poles=[.84])
+    controller_poles=[.92])
 
 
 def main(argv):
     if FLAGS.plot:
         R = numpy.matrix([[0.0], [500.0], [0.0]])
-        flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
+        flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=400)
         return 0
 
     if len(argv) != 5:
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 630d223..451788a 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -30,7 +30,7 @@
         self.controller_poles = controller_poles
 
 
-class VelocityFlywheel(control_loop.ControlLoop):
+class VelocityFlywheel(control_loop.HybridControlLoop):
     def __init__(self, params, name="Flywheel"):
         super(VelocityFlywheel, self).__init__(name=name)
         self.params = params
@@ -121,26 +121,44 @@
         self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
         self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
 
+
         # states
         # [position, velocity, voltage_error]
         self.C_unaugmented = self.C
         self.C = numpy.matrix(numpy.zeros((1, 3)))
         self.C[0:1, 0:2] = self.C_unaugmented
 
+        glog.debug('A_continuous %s' % str(self.A_continuous))
+        glog.debug('B_continuous %s' % str(self.B_continuous))
+        glog.debug('C %s' % str(self.C))
+
         self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
                                                    self.B_continuous, self.dt)
 
+        glog.debug('A %s' % str(self.A))
+        glog.debug('B %s' % str(self.B))
+
         q_pos = self.params.q_pos
         q_vel = self.params.q_vel
         q_voltage = self.params.q_voltage
-        self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+        self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
                                [0.0, (q_vel**2.0), 0.0],
                                [0.0, 0.0, (q_voltage**2.0)]])
 
         r_pos = self.params.r_pos
-        self.R = numpy.matrix([[(r_pos**2.0)]])
+        self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
 
-        self.KalmanGain, self.Q_steady = controls.kalman(
+        _, _, self.Q, self.R = controls.kalmd(
+            A_continuous=self.A_continuous,
+            B_continuous=self.B_continuous,
+            Q_continuous=self.Q_continuous,
+            R_continuous=self.R_continuous,
+            dt=self.dt)
+
+        glog.debug('Q_discrete %s' % (str(self.Q)))
+        glog.debug('R_discrete %s' % (str(self.R)))
+
+        self.KalmanGain, self.P_steady_state = controls.kalman(
             A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
         self.L = self.A * self.KalmanGain
 
@@ -155,7 +173,7 @@
         self.InitializeState()
 
 
-def PlotSpinup(params, goal, iterations=200):
+def PlotSpinup(params, goal, iterations=400):
     """Runs the flywheel plant with an initial condition and goal.
 
     Args:
@@ -210,21 +228,20 @@
 
         if observer_flywheel is not None:
             observer_flywheel.Y = flywheel.Y
-            observer_flywheel.CorrectObserver(U)
+            observer_flywheel.CorrectHybridObserver(U)
             offset.append(observer_flywheel.X_hat[2, 0])
 
         applied_U = U.copy()
-        if i > 100:
+        if i > 200:
             applied_U += 2
         flywheel.Update(applied_U)
 
         if observer_flywheel is not None:
-            observer_flywheel.PredictObserver(U)
+            observer_flywheel.PredictHybridObserver(U, flywheel.dt)
 
         t.append(initial_t + i * flywheel.dt)
         u.append(U[0, 0])
 
-        glog.debug('Time: %f', t[-1])
     pylab.subplot(3, 1, 1)
     pylab.plot(t, v, label='x')
     pylab.plot(t, x_hat, label='x_hat')
@@ -281,5 +298,9 @@
     loop_writer.Write(plant_files[0], plant_files[1])
 
     integral_loop_writer = control_loop.ControlLoopWriter(
-        'Integral' + name, integral_flywheels, namespaces=namespace)
+        'Integral' + name,
+        integral_flywheels,
+        namespaces=namespace,
+        plant_type='StateFeedbackHybridPlant',
+        observer_type='HybridKalman')
     integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index b4d8c84..7e75dce 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -31,13 +31,13 @@
 kHood = angular_system.AngularSystemParams(
     name='Hood',
     motor=control_loop.BAG(),
-    G=radians_per_turn,
-    J=0.08389,
-    q_pos=0.55,
-    q_vel=14.0,
+    G=radians_per_turn / (2.0 * numpy.pi),
+    J=4.0,
+    q_pos=0.15,
+    q_vel=5.0,
     kalman_q_pos=0.12,
-    kalman_q_vel=2.0,
-    kalman_q_voltage=2.5,
+    kalman_q_vel=10.0,
+    kalman_q_voltage=15.0,
     kalman_r_position=0.05)
 
 
@@ -47,6 +47,8 @@
         angular_system.PlotKick(kHood, R)
         angular_system.PlotMotion(kHood, R)
 
+    glog.info("Radians per turn: %f\n", radians_per_turn)
+
     # Write the generated constants out to a file.
     if len(argv) != 5:
         glog.fatal(
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index e276457..5a27afb 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -22,12 +22,12 @@
     name='Turret',
     motor=control_loop.Vex775Pro(),
     G=(6.0 / 60.0) * (26.0 / 150.0),
-    J=0.11,
-    q_pos=0.40,
-    q_vel=7.0,
+    J=0.20,
+    q_pos=0.30,
+    q_vel=4.5,
     kalman_q_pos=0.12,
-    kalman_q_vel=2.0,
-    kalman_q_voltage=3.0,
+    kalman_q_vel=10.0,
+    kalman_q_voltage=12.0,
     kalman_r_position=0.05)
 
 def main(argv):
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 77fb769..07c3d99 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -11,8 +11,12 @@
 namespace superstructure {
 namespace shooter {
 
-FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
-    : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
+FlywheelController::FlywheelController(
+    StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                      HybridKalman<3, 1, 1>> &&loop)
+    : loop_(new StateFeedbackLoop<3, 1, 1, double,
+                                  StateFeedbackHybridPlant<3, 1, 1>,
+                                  HybridKalman<3, 1, 1>>(std::move(loop))) {
   history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
       0, ::aos::monotonic_clock::epoch()));
   Y_.setZero();
@@ -26,6 +30,18 @@
 void FlywheelController::set_position(
     double current_position,
     const aos::monotonic_clock::time_point position_timestamp) {
+  // Project time forwards.
+  const int newest_history_position =
+      ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+  if (!first_) {
+    loop_->UpdateObserver(
+        loop_->U(),
+        position_timestamp - std::get<1>(history_[newest_history_position]));
+  } else {
+    first_ = false;
+  }
+
   // Update position in the model.
   Y_ << current_position;
 
@@ -34,6 +50,8 @@
       std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
                                                             position_timestamp);
   history_position_ = (history_position_ + 1) % kHistoryLength;
+
+  loop_->Correct(Y_);
 }
 
 double FlywheelController::voltage() const { return loop_->U(0, 0); }
@@ -45,22 +63,22 @@
     disabled = true;
   }
 
-  loop_->Correct(Y_);
-  loop_->Update(disabled);
+  loop_->UpdateController(disabled);
 }
 
 flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
     flatbuffers::FlatBufferBuilder *fbb) {
   // Compute the oldest point in the history.
-  const int oldest_history_position =
+  const int oldest_history_position = history_position_;
+  const int newest_history_position =
       ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
 
   const double total_loop_time = ::aos::time::DurationInSeconds(
-      std::get<1>(history_[history_position_]) -
+      std::get<1>(history_[newest_history_position]) -
       std::get<1>(history_[oldest_history_position]));
 
   const double distance_traveled =
-      std::get<0>(history_[history_position_]) -
+      std::get<0>(history_[newest_history_position]) -
       std::get<0>(history_[oldest_history_position]);
 
   // Compute the distance moved over that time period.
@@ -70,6 +88,7 @@
 
   builder.add_avg_angular_velocity(avg_angular_velocity_);
   builder.add_angular_velocity(loop_->X_hat(1, 0));
+  builder.add_voltage_error(loop_->X_hat(2, 0));
   builder.add_angular_velocity_goal(last_goal_);
   return builder.Finish();
 }
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index a3e9bdb..e130389 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -18,7 +18,9 @@
 // Handles the velocity control of each flywheel.
 class FlywheelController {
  public:
-  FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop);
+  FlywheelController(
+      StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                        HybridKalman<3, 1, 1>> &&loop);
 
   // Sets the velocity goal in radians/sec
   void set_goal(double angular_velocity_goal);
@@ -45,7 +47,10 @@
   // The current sensor measurement.
   Eigen::Matrix<double, 1, 1> Y_;
   // The control loop.
-  ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+  ::std::unique_ptr<
+      StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                        HybridKalman<3, 1, 1>>>
+      loop_;
 
   // History array for calculating a filtered angular velocity.
   static constexpr int kHistoryLength = 10;
@@ -59,6 +64,8 @@
 
   double last_goal_ = 0;
 
+  bool first_ = true;
+
   DISALLOW_COPY_AND_ASSIGN(FlywheelController);
 };
 
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 25f6a6a..a9f1c4c 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -12,7 +12,7 @@
 namespace shooter {
 
 namespace {
-const double kVelocityTolerance = 0.01;
+const double kVelocityTolerance = 20.0;
 }  // namespace
 
 Shooter::Shooter()
@@ -46,16 +46,18 @@
   accelerator_right_.set_position(position->theta_accelerator_right(),
                                   position_timestamp);
 
-  finisher_.Update(output == nullptr);
-  accelerator_left_.Update(output == nullptr);
-  accelerator_right_.Update(output == nullptr);
-
   // Update goal.
   if (goal) {
     finisher_.set_goal(goal->velocity_finisher());
     accelerator_left_.set_goal(goal->velocity_accelerator());
     accelerator_right_.set_goal(goal->velocity_accelerator());
+  }
 
+  finisher_.Update(output == nullptr);
+  accelerator_left_.Update(output == nullptr);
+  accelerator_right_.Update(output == nullptr);
+
+  if (goal) {
     if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
         goal->velocity_accelerator() > kVelocityTolerance) {
       ready_ = true;
diff --git a/y2020/control_loops/superstructure/shooter_plot.pb b/y2020/control_loops/superstructure/shooter_plot.pb
index 01a1e20..16ab5f4 100644
--- a/y2020/control_loops/superstructure/shooter_plot.pb
+++ b/y2020/control_loops/superstructure/shooter_plot.pb
@@ -24,19 +24,87 @@
     line {
       y_signal {
         channel: "Status"
-        field: "hood.position"
+        field: "shooter.accelerator_left.avg_angular_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.accelerator_right.avg_angular_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.accelerator_left.angular_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.accelerator_right.angular_velocity"
       }
     }
     line {
       y_signal {
         channel: "Goal"
-        field: "hood.unsafe_goal"
+        field: "shooter.velocity_accelerator"
       }
     }
     line {
       y_signal {
-        channel: "Position"
-        field: "hood.encoder"
+        channel: "Status"
+        field: "shooter.finisher.avg_angular_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.finisher.angular_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Goal"
+        field: "shooter.velocity_finisher"
+      }
+    }
+  }
+  axes {
+    line {
+      y_signal {
+        channel: "Output"
+        field: "accelerator_left_voltage"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Output"
+        field: "accelerator_right_voltage"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.accelerator_left.voltage_error"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.accelerator_right.voltage_error"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Output"
+        field: "finisher_voltage"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Status"
+        field: "shooter.finisher.voltage_error"
       }
     }
     ylabel: "hood position"
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 9a6c3cd..5dfd2d1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -42,6 +42,30 @@
                     output != nullptr ? &(output_struct.hood_voltage) : nullptr,
                     status->fbb());
 
+  if (unsafe_goal != nullptr) {
+    if (unsafe_goal->shooting() &&
+        shooting_start_time_ == aos::monotonic_clock::min_time) {
+      shooting_start_time_ = position_timestamp;
+    }
+
+    if (unsafe_goal->shooting()) {
+      constexpr std::chrono::milliseconds kPeriod =
+          std::chrono::milliseconds(250);
+      if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
+          kPeriod) {
+        intake_joint_.set_min_position(-0.25);
+      } else {
+        intake_joint_.set_min_position(-0.75);
+      }
+    } else {
+      intake_joint_.clear_min_position();
+    }
+
+    if (!unsafe_goal->shooting()) {
+      shooting_start_time_ = aos::monotonic_clock::min_time;
+    }
+  }
+
   flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
       intake_joint_.Iterate(
           unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
@@ -64,19 +88,37 @@
 
   climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
 
+  const AbsoluteEncoderProfiledJointStatus *const hood_status =
+      GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
+
+  const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
+      GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
+
+  if (output != nullptr) {
+    // Friction is a pain and putting a really high burden on the integrator.
+    const double turret_velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+    output_struct.turret_voltage +=
+        std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
+                   kTurretFrictionVoltageLimit);
+
+    // Friction is a pain and putting a really high burden on the integrator.
+    const double hood_velocity_sign = hood_status->velocity() * kHoodFrictionGain;
+    output_struct.hood_voltage +=
+        std::clamp(hood_velocity_sign, -kHoodFrictionVoltageLimit,
+                   kHoodFrictionVoltageLimit);
+
+    // And dither the output.
+    time_ += 0.00505;
+    output_struct.hood_voltage += 1.3 * std::sin(time_ * 2.0 * M_PI * 30.0);
+  }
+
   bool zeroed;
   bool estopped;
 
   {
-    const AbsoluteEncoderProfiledJointStatus *const hood_status =
-        GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
-
     const AbsoluteEncoderProfiledJointStatus *const intake_status =
         GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
 
-    const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
-        GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
-
     zeroed = hood_status->zeroed() && intake_status->zeroed() &&
              turret_status->zeroed();
     estopped = hood_status->estopped() || intake_status->estopped() ||
@@ -97,13 +139,21 @@
 
   if (output != nullptr) {
     if (unsafe_goal) {
-        output_struct.washing_machine_spinner_voltage = 6.0;
+      output_struct.washing_machine_spinner_voltage = 0.0;
       if (unsafe_goal->shooting()) {
-        output_struct.feeder_voltage = 6.0;
+        if (shooter_.ready() &&
+            unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
+            unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+          output_struct.feeder_voltage = 9.0;
+        } else {
+          output_struct.feeder_voltage = 0.0;
+        }
+        output_struct.washing_machine_spinner_voltage = 5.0;
+        output_struct.intake_roller_voltage = 3.0;
       } else {
         output_struct.feeder_voltage = 0.0;
+        output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
       }
-      output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
     } else {
       output_struct.intake_roller_voltage = 0.0;
     }
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index d8ce917..1526610 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -21,6 +21,14 @@
   explicit Superstructure(::aos::EventLoop *event_loop,
                           const ::std::string &name = "/superstructure");
 
+  // Terms to control the velocity gain for the friction compensation, and the
+  // voltage cap.
+  static constexpr double kTurretFrictionGain = 10.0;
+  static constexpr double kTurretFrictionVoltageLimit = 1.5;
+
+  static constexpr double kHoodFrictionGain = 40.0;
+  static constexpr double kHoodFrictionVoltageLimit = 1.5;
+
   using PotAndAbsoluteEncoderSubsystem =
       ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
           ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
@@ -47,6 +55,12 @@
   shooter::Shooter shooter_;
 
   Climber climber_;
+
+  aos::monotonic_clock::time_point shooting_start_time_ =
+      aos::monotonic_clock::min_time;
+
+  double time_ = 0.0;
+
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };
 
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index f066ab0..c7fe643 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -27,9 +27,7 @@
   // Positive is rollers intaking to Washing Machine.
   roller_voltage:float;
 
-  // 0 = facing the front of the robot. Positive rotates counterclockwise.
-  // TODO(Kai): Define which wrap of the shooter is 0 if it can rotate past
-  // forward more than once.
+  // 0 = facing the back of the robot. Positive rotates counterclockwise.
   turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
 
   // Only applies if shooter_tracking = false.
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index f68db08..6e4e08b 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -235,17 +235,29 @@
     EXPECT_NEAR(superstructure_output_fetcher_->turret_voltage(), 0.0,
                 voltage_check_turret);
 
+    // Invert the friction model.
+    const double hood_velocity_sign =
+        hood_plant_->X(1) * Superstructure::kHoodFrictionGain;
     ::Eigen::Matrix<double, 1, 1> hood_U;
     hood_U << superstructure_output_fetcher_->hood_voltage() +
-                  hood_plant_->voltage_offset();
+                  hood_plant_->voltage_offset() -
+                  std::clamp(hood_velocity_sign,
+                             -Superstructure::kHoodFrictionVoltageLimit,
+                             Superstructure::kHoodFrictionVoltageLimit);
 
     ::Eigen::Matrix<double, 1, 1> intake_U;
     intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
                     intake_plant_->voltage_offset();
 
+    // Invert the friction model.
+    const double turret_velocity_sign =
+        turret_plant_->X(1) * Superstructure::kTurretFrictionGain;
     ::Eigen::Matrix<double, 1, 1> turret_U;
     turret_U << superstructure_output_fetcher_->turret_voltage() +
-                    turret_plant_->voltage_offset();
+                    turret_plant_->voltage_offset() -
+                    std::clamp(turret_velocity_sign,
+                               -Superstructure::kTurretFrictionVoltageLimit,
+                               Superstructure::kTurretFrictionVoltageLimit);
 
     ::Eigen::Matrix<double, 1, 1> accelerator_left_U;
     accelerator_left_U
@@ -667,7 +679,8 @@
     ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
   superstructure_plant_.set_peak_hood_velocity(23.0);
-  superstructure_plant_.set_peak_hood_acceleration(0.2);
+  // 30 hz sin wave on the hood causes acceleration to be ignored.
+  superstructure_plant_.set_peak_hood_acceleration(5.5);
 
   superstructure_plant_.set_peak_intake_velocity(23.0);
   superstructure_plant_.set_peak_intake_acceleration(0.2);
@@ -750,7 +763,7 @@
   }
 
   // Give it a lot of time to get there.
-  RunFor(chrono::seconds(9));
+  RunFor(chrono::seconds(15));
 
   VerifyNearGoal();
 }
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index e8f7637..0b12d33 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -14,6 +14,9 @@
   // The target speed selected by the lookup table or from manual override
   // Can be compared to velocity to determine if ready.
   angular_velocity_goal:double;
+
+  // Voltage error.
+  voltage_error:double;
 }
 
 table ShooterStatus {
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 75a4b26..026a50b 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -96,7 +96,13 @@
     }
 
     if (data.IsPressed(kIntakeExtend)) {
-      intake_pos = 1.0;
+      intake_pos = 1.2;
+      roller_speed = 9.0f;
+    }
+
+    if (superstructure_status_fetcher_.get() &&
+        superstructure_status_fetcher_->intake()->position() > -0.5) {
+      roller_speed = std::max(roller_speed, 6.0f);
     }
 
     if (data.IsPressed(kFeed)) {
@@ -119,7 +125,7 @@
       flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
           hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
               *builder.fbb(), hood_pos,
-              CreateProfileParameters(*builder.fbb(), 0.5, 1.0));
+              CreateProfileParameters(*builder.fbb(), 0.7, 3.0));
 
       flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
           intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index b643fcb..5e65387 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -31,14 +31,14 @@
     srcs = [
         "camera_reader.cc",
     ],
+    data = [
+        "//y2020:config.json",
+    ],
     restricted_to = [
         "//tools:k8",
         "//tools:armhf-debian",
     ],
     visibility = ["//y2020:__subpackages__"],
-    data = [
-        "//y2020:config.json",
-    ],
     deps = [
         ":v4l2_reader",
         ":vision_fbs",
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index d45ec3f..c47e42b 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -76,6 +76,8 @@
                             const std::vector<std::vector<cv::DMatch>> &matches,
                             const std::vector<cv::Mat> &camera_target_list,
                             const std::vector<cv::Mat> &field_camera_list,
+                            const std::vector<cv::Point2f> &target_point_vector,
+                            const std::vector<float> &target_radius_vector,
                             aos::Sender<sift::ImageMatchResult> *result_sender,
                             bool send_details);
 
@@ -113,6 +115,17 @@
         ->field_to_target();
   }
 
+  void TargetLocation(int training_image_index, cv::Point2f &target_location,
+                      float &target_radius) {
+    target_location.x =
+        training_data_->images()->Get(training_image_index)->target_point_x();
+    target_location.y =
+        training_data_->images()->Get(training_image_index)->target_point_y();
+    target_radius = training_data_->images()
+                        ->Get(training_image_index)
+                        ->target_point_radius();
+  }
+
   int number_training_images() const {
     return training_data_->images()->size();
   }
@@ -193,6 +206,8 @@
     const std::vector<std::vector<cv::DMatch>> &matches,
     const std::vector<cv::Mat> &camera_target_list,
     const std::vector<cv::Mat> &field_camera_list,
+    const std::vector<cv::Point2f> &target_point_vector,
+    const std::vector<float> &target_radius_vector,
     aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
   auto builder = result_sender->MakeBuilder();
   const auto camera_calibration_offset =
@@ -234,6 +249,9 @@
     pose_builder.add_camera_to_target(transform_offset);
     pose_builder.add_field_to_camera(fc_transform_offset);
     pose_builder.add_field_to_target(field_to_target_offset);
+    pose_builder.add_query_target_point_x(target_point_vector[i].x);
+    pose_builder.add_query_target_point_y(target_point_vector[i].y);
+    pose_builder.add_query_target_point_radius(target_radius_vector[i]);
     camera_poses.emplace_back(pose_builder.Finish());
   }
   const auto camera_poses_offset = builder.fbb()->CreateVector(camera_poses);
@@ -317,8 +335,12 @@
   std::vector<cv::Mat> camera_target_list;
   std::vector<cv::Mat> field_camera_list;
 
-  std::vector<PerImageMatches> per_image_good_matches;
+  // Rebuild the matches and store them here
   std::vector<std::vector<cv::DMatch>> all_good_matches;
+  // Build list of target point and radius for each good match
+  std::vector<cv::Point2f> target_point_vector;
+  std::vector<float> target_radius_vector;
+
   // Iterate through matches for each training image
   for (size_t i = 0; i < per_image_matches.size(); ++i) {
     const PerImageMatches &per_image = per_image_matches[i];
@@ -343,8 +365,8 @@
 
     VLOG(2) << "Number of matches after homography: " << cv::countNonZero(mask)
             << "\n";
-    // Fill our match info for each good match
-    // TODO<Jim>: Could probably optimize some of the copies here
+
+    // Fill our match info for each good match based on homography result
     PerImageMatches per_image_good_match;
     CHECK_EQ(per_image.training_points.size(),
              (unsigned long)mask.size().height);
@@ -359,22 +381,46 @@
           static_cast<std::vector<cv::DMatch>>(*per_image.matches[j]));
 
       // Fill out the data for matches per image that made it past
-      // homography check
+      // homography check, for later use
       per_image_good_match.matches.push_back(per_image.matches[j]);
       per_image_good_match.training_points.push_back(
           per_image.training_points[j]);
       per_image_good_match.training_points_3d.push_back(
           per_image.training_points_3d[j]);
       per_image_good_match.query_points.push_back(per_image.query_points[j]);
-      per_image_good_match.homography = homography;
     }
-    per_image_good_matches.push_back(per_image_good_match);
 
-    // TODO: Use homography to compute target point on query image
+    // Returns from opencv are doubles (CV_64F), which don't play well
+    // with our floats
+    homography.convertTo(homography, CV_32F);
+    per_image_good_match.homography = homography;
+
+    CHECK_GT(per_image_good_match.matches.size(), 0u);
+
+    // Collect training target location, so we can map it to matched image
+    cv::Point2f target_point;
+    float target_radius;
+    TargetLocation((*(per_image_good_match.matches[0]))[0].imgIdx, target_point,
+                   target_radius);
+
+    // Store target_point in vector for use by perspectiveTransform
+    std::vector<cv::Point2f> src_target_pt;
+    src_target_pt.push_back(target_point);
+    std::vector<cv::Point2f> query_target_pt;
+
+    cv::perspectiveTransform(src_target_pt, query_target_pt, homography);
+
+    float query_target_radius =
+        target_radius *
+        abs(homography.at<float>(0, 0) + homography.at<float>(1, 1)) / 2.;
+
+    CHECK_EQ(query_target_pt.size(), 1u);
+    target_point_vector.push_back(query_target_pt[0]);
+    target_radius_vector.push_back(query_target_radius);
 
     // Pose transformations (rotations and translations) for various
     // coordinate frames.  R_X_Y_vec is the Rodrigues (angle-axis)
-    // representation the 3x3 rotation R_X_Y from frame X to frame Y
+    // representation of the 3x3 rotation R_X_Y from frame X to frame Y
 
     // Tranform from camera to target frame
     cv::Mat R_camera_target_vec, R_camera_target, T_camera_target;
@@ -444,10 +490,12 @@
   // debugging(features), and one smaller
   SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
                        camera_target_list, field_camera_list,
+                       target_point_vector, target_radius_vector,
                        &detailed_result_sender_, true);
   SendImageMatchResult(image, keypoints, descriptors, all_good_matches,
-                       camera_target_list, field_camera_list, &result_sender_,
-                       false);
+                       camera_target_list, field_camera_list,
+                       target_point_vector, target_radius_vector,
+                       &result_sender_, false);
 }
 
 void CameraReader::ReadImage() {
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 43ea152..3e2daaf 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -133,6 +133,12 @@
 
   // The pose of the camera in the field coordinate frame
   field_to_camera:TransformationMatrix;
+
+  // 2D image coordinate representing target location on the matched image
+  query_target_point_x:float;
+  query_target_point_y:float;
+  // Perceived radius of target circle
+  query_target_point_radius:float;
 }
 
 table ImageMatchResult {
@@ -153,10 +159,6 @@
 
   // Information about the camera which took this image.
   camera_calibration:CameraCalibration;
-
-  // 2D image coordinate representing target location on the matched image
-  target_point_x:float;
-  target_point_y:float;
 }
 
 root_type ImageMatchResult;
diff --git a/y2020/vision/sift/sift_training.fbs b/y2020/vision/sift/sift_training.fbs
index d4fa740..12ad9b4 100644
--- a/y2020/vision/sift/sift_training.fbs
+++ b/y2020/vision/sift/sift_training.fbs
@@ -14,6 +14,8 @@
   // 2D image coordinate representing target location on the training image
   target_point_x:float;
   target_point_y:float;
+  // Radius of target circle
+  target_point_radius:float;
 }
 
 // Represents the information used to match incoming images against.
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
index 157f9e6..9894ee2 100644
--- a/y2020/vision/tools/python_code/define_training_data.py
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -18,7 +18,7 @@
     global current_mouse
     current_mouse = (x, y)
     if event == cv2.EVENT_LBUTTONUP:
-        glog.debug("Adding point at %d, %d" % (x,y))
+        glog.debug("Adding point at %d, %d" % (x, y))
         point_list.append([x, y])
     pass
 
@@ -143,20 +143,22 @@
 
     return point_list
 
+
 # Determine whether a given point lies within (or on border of) a set of polygons
 # Return true if it does
 def point_in_polygons(point, polygons):
     for poly in polygons:
         np_poly = np.asarray(poly)
         dist = cv2.pointPolygonTest(np_poly, (point[0], point[1]), True)
-        if dist >=0:
+        if dist >= 0:
             return True
 
     return False
 
+
 ## Filter keypoints by polygons
 def filter_keypoints_by_polygons(keypoint_list, descriptor_list, polygons):
-    # TODO: Need to make sure we've got the right numpy array / list 
+    # TODO: Need to make sure we've got the right numpy array / list
     keep_keypoint_list = []
     keep_descriptor_list = []
     reject_keypoint_list = []
@@ -166,7 +168,8 @@
 
     # For now, pretend keypoints are just points
     for i in range(len(keypoint_list)):
-        if point_in_polygons((keypoint_list[i].pt[0], keypoint_list[i].pt[1]), polygons):
+        if point_in_polygons((keypoint_list[i].pt[0], keypoint_list[i].pt[1]),
+                             polygons):
             keep_list.append(i)
         else:
             reject_list.append(i)
@@ -175,37 +178,42 @@
     reject_keypoint_list = [keypoint_list[kp_ind] for kp_ind in reject_list]
     # Allow function to be called with no descriptors, and just return empty list
     if descriptor_list is not None:
-        keep_descriptor_list = descriptor_list[keep_list,:]
-        reject_descriptor_list = descriptor_list[reject_list,:]
+        keep_descriptor_list = descriptor_list[keep_list, :]
+        reject_descriptor_list = descriptor_list[reject_list, :]
 
     return keep_keypoint_list, keep_descriptor_list, reject_keypoint_list, reject_descriptor_list, keep_list
 
+
 # Helper function that appends a column of ones to a list (of 2d points)
 def append_ones(point_list):
-    return np.hstack([point_list, np.ones((len(point_list),1))])
+    return np.hstack([point_list, np.ones((len(point_list), 1))])
+
 
 # Given a list of 2d points and thei corresponding 3d locations, compute map
 # between them.
 # pt_3d = (pt_2d, 1) * reprojection_map
 # TODO: We should really look at residuals to see if things are messed up
 def compute_reprojection_map(polygon_2d, polygon_3d):
-    pts_2d = np.asarray(np.float32(polygon_2d)).reshape(-1,2)
+    pts_2d = np.asarray(np.float32(polygon_2d)).reshape(-1, 2)
     pts_2d_lstsq = append_ones(pts_2d)
-    pts_3d_lstsq = np.asarray(np.float32(polygon_3d)).reshape(-1,3)
+    pts_3d_lstsq = np.asarray(np.float32(polygon_3d)).reshape(-1, 3)
 
     reprojection_map = np.linalg.lstsq(pts_2d_lstsq, pts_3d_lstsq, rcond=-1)[0]
 
     return reprojection_map
 
+
 # Given set of keypoints (w/ 2d location), a reprojection map, and a polygon
 # that defines regions for where this is valid
 # Returns a numpy array of 3d locations the same size as the keypoint list
 def compute_3d_points(keypoint_2d_list, reprojection_map):
-    pt_2d_lstsq = append_ones(np.asarray(np.float32(keypoint_2d_list)).reshape(-1,2))
+    pt_2d_lstsq = append_ones(
+        np.asarray(np.float32(keypoint_2d_list)).reshape(-1, 2))
     pt_3d = pt_2d_lstsq.dot(reprojection_map)
 
     return pt_3d
 
+
 # Given 2d and 3d locations, and camera location and projection model,
 # display locations on an image
 def visualize_reprojections(img, pts_2d, pts_3d, cam_mat, distortion_coeffs):
@@ -246,11 +254,8 @@
 def sample_define_points_by_list_usage():
     image = cv2.imread("test_images/train_power_port_red.png")
 
-    test_points = [(451, 679), (451, 304),
-                   (100, 302), (451, 74),
-                   (689, 74), (689, 302),
-                   (689, 679)]
+    test_points = [(451, 679), (451, 304), (100, 302), (451, 74), (689, 74),
+                   (689, 302), (689, 679)]
 
     polygon_list = define_points_by_list(image, test_points)
     glog.debug(polygon_list)
-
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 651efe2..6db3155 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -107,28 +107,27 @@
             fbb)
 
         # Create the TrainingImage feature vector
-        TrainingImage.TrainingImageStartFeaturesVector(
-            fbb, len(features_vector))
+        TrainingImage.TrainingImageStartFeaturesVector(fbb,
+                                                       len(features_vector))
         for feature in reversed(features_vector):
             fbb.PrependUOffsetTRelative(feature)
         features_vector_offset = fbb.EndVector(len(features_vector))
 
         # Add the TrainingImage data
         TrainingImage.TrainingImageStart(fbb)
-        TrainingImage.TrainingImageAddFeatures(fbb,
-                                                       features_vector_offset)
-        TrainingImage.TrainingImageAddFieldToTarget(
-            fbb, transformation_mat_offset)
+        TrainingImage.TrainingImageAddFeatures(fbb, features_vector_offset)
+        TrainingImage.TrainingImageAddFieldToTarget(fbb,
+                                                    transformation_mat_offset)
         TrainingImage.TrainingImageAddTargetPointX(
             fbb, target_data.target_point_2d[0][0][0])
         TrainingImage.TrainingImageAddTargetPointY(
             fbb, target_data.target_point_2d[0][0][1])
-        images_vector.append(
-            TrainingImage.TrainingImageEnd(fbb))
+        TrainingImage.TrainingImageAddTargetPointRadius(
+            fbb, target_data.target_radius)
+        images_vector.append(TrainingImage.TrainingImageEnd(fbb))
 
     # Create and add Training Data of all targets
-    TrainingData.TrainingDataStartImagesVector(fbb,
-                                                     len(images_vector))
+    TrainingData.TrainingDataStartImagesVector(fbb, len(images_vector))
     for training_image in reversed(images_vector):
         fbb.PrependUOffsetTRelative(training_image)
     images_vector_table = fbb.EndVector(len(images_vector))
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index 4c3f36b..eae358c 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -16,6 +16,9 @@
 USE_BAZEL = True
 VISUALIZE_KEYPOINTS = False
 
+# For now, just have a 32 pixel radius, based on original training image
+target_radius_default = 32.
+
 
 def bazel_name_fix(filename):
     ret_name = filename
@@ -44,6 +47,7 @@
         self.target_rotation = None
         self.target_position = None
         self.target_point_2d = None
+        self.target_radius = target_radius_default
 
     def extract_features(self, feature_extractor=None):
         if feature_extractor is None:
@@ -73,8 +77,8 @@
             # Filter and project points for each polygon in the list
             filtered_keypoints, _, _, _, keep_list = dtd.filter_keypoints_by_polygons(
                 keypoint_list, None, [self.polygon_list[poly_ind]])
-            glog.info("Filtering kept %d of %d features" % (len(keep_list),
-                                                            len(keypoint_list)))
+            glog.info("Filtering kept %d of %d features" %
+                      (len(keep_list), len(keypoint_list)))
             filtered_point_array = np.asarray(
                 [(keypoint.pt[0], keypoint.pt[1])
                  for keypoint in filtered_keypoints]).reshape(-1, 2)
@@ -114,7 +118,8 @@
     wing_angle = 20. * math.pi / 180.
 
     # Pick the target center location at halfway between top and bottom of the top panel
-    power_port_target_height = (power_port_total_height + power_port_bottom_wing_height) / 2.
+    power_port_target_height = (
+        power_port_total_height + power_port_bottom_wing_height) / 2.
 
     ###
     ### Red Power Port
@@ -132,32 +137,32 @@
 
     # These are "virtual" 3D points based on the expected geometry
     power_port_red_main_panel_polygon_points_3d = [
-        (-field_length/2., power_port_edge_y, 0.),
-        (-field_length/2., power_port_edge_y,
-         power_port_bottom_wing_height),
-        (-field_length/2., power_port_edge_y
-         - power_port_wing_width, power_port_bottom_wing_height),
-        (-field_length/2., power_port_edge_y,
-         power_port_total_height),
-        (-field_length/2., power_port_edge_y + power_port_width,
-         power_port_total_height),
-        (-field_length/2., power_port_edge_y + power_port_width,
-         power_port_bottom_wing_height),
-        (-field_length/2., power_port_edge_y + power_port_width, 0.)
+        (-field_length / 2., power_port_edge_y,
+         0.), (-field_length / 2., power_port_edge_y,
+               power_port_bottom_wing_height),
+        (-field_length / 2., power_port_edge_y - power_port_wing_width,
+         power_port_bottom_wing_height), (-field_length / 2.,
+                                          power_port_edge_y,
+                                          power_port_total_height),
+        (-field_length / 2., power_port_edge_y + power_port_width,
+         power_port_total_height), (-field_length / 2.,
+                                    power_port_edge_y + power_port_width,
+                                    power_port_bottom_wing_height),
+        (-field_length / 2., power_port_edge_y + power_port_width, 0.)
     ]
 
     power_port_red_wing_panel_polygon_points_2d = [(689, 74), (1022, 302),
                                                    (689, 302)]
     # These are "virtual" 3D points based on the expected geometry
     power_port_red_wing_panel_polygon_points_3d = [
-        (-field_length/2., power_port_edge_y + power_port_width,
+        (-field_length / 2., power_port_edge_y + power_port_width,
          power_port_total_height),
-        (-field_length/2. + power_port_wing_width * math.sin(wing_angle),
-         power_port_edge_y + power_port_width
-         + power_port_wing_width * math.cos(wing_angle),
-         power_port_bottom_wing_height),
-        (-field_length/2., power_port_edge_y + power_port_width,
-         power_port_bottom_wing_height)
+        (-field_length / 2. + power_port_wing_width * math.sin(wing_angle),
+         power_port_edge_y + power_port_width +
+         power_port_wing_width * math.cos(wing_angle),
+         power_port_bottom_wing_height), (-field_length / 2.,
+                                          power_port_edge_y + power_port_width,
+                                          power_port_bottom_wing_height)
     ]
 
     # Populate the red power port
@@ -173,16 +178,17 @@
     # and the skew to target is zero
     ideal_power_port_red.target_rotation = -np.identity(3, np.double)
     ideal_power_port_red.target_rotation[2][2] = 1.
-    ideal_power_port_red.target_position = np.array([-field_length/2.,
-                                                     power_port_edge_y + power_port_width/2.,
-                                                     power_port_target_height])
+    ideal_power_port_red.target_position = np.array([
+        -field_length / 2., power_port_edge_y + power_port_width / 2.,
+        power_port_target_height
+    ])
 
     # Target point on the image -- needs to match training image
     # These are manually captured by examining the images,
     # and entering the pixel values from the target center for each image.
     # These are currently only used for visualization of the target
-    ideal_power_port_red.target_point_2d = np.float32([[570,192]]).reshape(-1,1,2)  # train_power_port_red.png
-
+    ideal_power_port_red.target_point_2d = np.float32([[570, 192]]).reshape(
+        -1, 1, 2)  # train_power_port_red.png
     # np.float32([[305, 97]]).reshape(-1, 1, 2),  #train_power_port_red_webcam.png
 
     # Add the ideal 3D target to our list
@@ -192,6 +198,8 @@
         'test_images/train_power_port_red_webcam.png')
     training_target_power_port_red.target_rotation = ideal_power_port_red.target_rotation
     training_target_power_port_red.target_position = ideal_power_port_red.target_position
+    training_target_power_port_red.target_radius = target_radius_default
+
     training_target_list.append(training_target_power_port_red)
 
     ###
@@ -207,10 +215,11 @@
 
     # These are "virtual" 3D points based on the expected geometry
     loading_bay_red_polygon_points_3d = [
-        (field_length/2., loading_bay_edge_y + loading_bay_width, 0.),
-        (field_length/2., loading_bay_edge_y + loading_bay_width, loading_bay_height),
-        (field_length/2., loading_bay_edge_y, loading_bay_height),
-        (field_length/2., loading_bay_edge_y, 0.)
+        (field_length / 2., loading_bay_edge_y + loading_bay_width, 0.),
+        (field_length / 2., loading_bay_edge_y + loading_bay_width,
+         loading_bay_height), (field_length / 2., loading_bay_edge_y,
+                               loading_bay_height), (field_length / 2.,
+                                                     loading_bay_edge_y, 0.)
     ]
 
     ideal_loading_bay_red.polygon_list.append(
@@ -219,16 +228,19 @@
         loading_bay_red_polygon_points_3d)
     # Location of target
     ideal_loading_bay_red.target_rotation = np.identity(3, np.double)
-    ideal_loading_bay_red.target_position = np.array([field_length/2.,
-                                                     loading_bay_edge_y + loading_bay_width/2.,
-                                                      loading_bay_height/2.])
-    ideal_loading_bay_red.target_point_2d = np.float32([[366, 236]]).reshape(-1, 1, 2)  # train_loading_bay_red.png
+    ideal_loading_bay_red.target_position = np.array([
+        field_length / 2., loading_bay_edge_y + loading_bay_width / 2.,
+        loading_bay_height / 2.
+    ])
+    ideal_loading_bay_red.target_point_2d = np.float32([[366, 236]]).reshape(
+        -1, 1, 2)  # train_loading_bay_red.png
 
     ideal_target_list.append(ideal_loading_bay_red)
     training_target_loading_bay_red = TargetData(
         'test_images/train_loading_bay_red.png')
     training_target_loading_bay_red.target_rotation = ideal_loading_bay_red.target_rotation
     training_target_loading_bay_red.target_position = ideal_loading_bay_red.target_position
+    training_target_loading_bay_red.target_radius = target_radius_default
     training_target_list.append(training_target_loading_bay_red)
 
     ###
@@ -246,31 +258,31 @@
 
     # These are "virtual" 3D points based on the expected geometry
     power_port_blue_main_panel_polygon_points_3d = [
-        (field_length/2., -power_port_edge_y, 0.),
-        (field_length/2., -power_port_edge_y,
-         power_port_bottom_wing_height),
-        (field_length/2., -power_port_edge_y + power_port_wing_width,
-         power_port_bottom_wing_height),
-        (field_length/2., -power_port_edge_y,
-         power_port_total_height),
-        (field_length/2., -power_port_edge_y - power_port_width,
-         power_port_total_height),
-        (field_length/2., -power_port_edge_y - power_port_width,
-         power_port_bottom_wing_height),
-        (field_length/2., -power_port_edge_y - power_port_width, 0.)
+        (field_length / 2., -power_port_edge_y,
+         0.), (field_length / 2., -power_port_edge_y,
+               power_port_bottom_wing_height),
+        (field_length / 2., -power_port_edge_y + power_port_wing_width,
+         power_port_bottom_wing_height), (field_length / 2.,
+                                          -power_port_edge_y,
+                                          power_port_total_height),
+        (field_length / 2., -power_port_edge_y - power_port_width,
+         power_port_total_height), (field_length / 2.,
+                                    -power_port_edge_y - power_port_width,
+                                    power_port_bottom_wing_height),
+        (field_length / 2., -power_port_edge_y - power_port_width, 0.)
     ]
 
     power_port_blue_wing_panel_polygon_points_2d = [(692, 50), (1047, 285),
                                                     (692, 285)]
     # These are "virtual" 3D points based on the expected geometry
     power_port_blue_wing_panel_polygon_points_3d = [
-        (field_length/2., -power_port_edge_y - power_port_width,
+        (field_length / 2., -power_port_edge_y - power_port_width,
          power_port_total_height),
-        (field_length/2. - power_port_wing_width * math.sin(wing_angle),
+        (field_length / 2. - power_port_wing_width * math.sin(wing_angle),
          -power_port_edge_y - power_port_width -
          power_port_wing_width * math.cos(wing_angle),
          power_port_bottom_wing_height),
-        (field_length/2., -power_port_edge_y - power_port_width,
+        (field_length / 2., -power_port_edge_y - power_port_width,
          power_port_bottom_wing_height)
     ]
 
@@ -282,16 +294,19 @@
 
     # Location of target.  Rotation is pointing in -x direction
     ideal_power_port_blue.target_rotation = np.identity(3, np.double)
-    ideal_power_port_blue.target_position = np.array([field_length/2.,
-                                                     -power_port_edge_y - power_port_width/2.,
-                                                      power_port_target_height])
-    ideal_power_port_blue.target_point_2d = np.float32([[567, 180]]).reshape(-1, 1, 2)  # train_power_port_blue.png
+    ideal_power_port_blue.target_position = np.array([
+        field_length / 2., -power_port_edge_y - power_port_width / 2.,
+        power_port_target_height
+    ])
+    ideal_power_port_blue.target_point_2d = np.float32([[567, 180]]).reshape(
+        -1, 1, 2)  # train_power_port_blue.png
 
     ideal_target_list.append(ideal_power_port_blue)
     training_target_power_port_blue = TargetData(
         'test_images/train_power_port_blue.png')
     training_target_power_port_blue.target_rotation = ideal_power_port_blue.target_rotation
     training_target_power_port_blue.target_position = ideal_power_port_blue.target_position
+    training_target_power_port_blue.target_radius = target_radius_default
     training_target_list.append(training_target_power_port_blue)
 
     ###
@@ -308,10 +323,11 @@
 
     # These are "virtual" 3D points based on the expected geometry
     loading_bay_blue_polygon_points_3d = [
-        (-field_length/2., -loading_bay_edge_y - loading_bay_width, 0.),
-        (-field_length/2., -loading_bay_edge_y - loading_bay_width, loading_bay_height),
-        (-field_length/2., -loading_bay_edge_y, loading_bay_height),
-        (-field_length/2., -loading_bay_edge_y, 0.)
+        (-field_length / 2., -loading_bay_edge_y - loading_bay_width, 0.),
+        (-field_length / 2., -loading_bay_edge_y - loading_bay_width,
+         loading_bay_height), (-field_length / 2., -loading_bay_edge_y,
+                               loading_bay_height), (-field_length / 2.,
+                                                     -loading_bay_edge_y, 0.)
     ]
 
     ideal_loading_bay_blue.polygon_list.append(
@@ -322,16 +338,19 @@
     # Location of target
     ideal_loading_bay_blue.target_rotation = -np.identity(3, np.double)
     ideal_loading_bay_blue.target_rotation[2][2] = 1.
-    ideal_loading_bay_blue.target_position = np.array([-field_length/2.,
-                                                     -loading_bay_edge_y - loading_bay_width/2.,
-                                                       loading_bay_height/2.])
-    ideal_loading_bay_blue.target_point_2d = np.float32([[366, 236]]).reshape(-1, 1, 2)  # train_loading_bay_blue.png
+    ideal_loading_bay_blue.target_position = np.array([
+        -field_length / 2., -loading_bay_edge_y - loading_bay_width / 2.,
+        loading_bay_height / 2.
+    ])
+    ideal_loading_bay_blue.target_point_2d = np.float32([[366, 236]]).reshape(
+        -1, 1, 2)  # train_loading_bay_blue.png
 
     ideal_target_list.append(ideal_loading_bay_blue)
     training_target_loading_bay_blue = TargetData(
         'test_images/train_loading_bay_blue.png')
     training_target_loading_bay_blue.target_rotation = ideal_loading_bay_blue.target_rotation
     training_target_loading_bay_blue.target_position = ideal_loading_bay_blue.target_position
+    training_target_loading_bay_blue.target_radius = target_radius_default
     training_target_list.append(training_target_loading_bay_blue)
 
     # Create feature extractor
@@ -341,7 +360,8 @@
     camera_params = camera_definition.web_cam_params
 
     for ideal_target in ideal_target_list:
-        glog.info("\nPreparing target for image %s" % ideal_target.image_filename)
+        glog.info(
+            "\nPreparing target for image %s" % ideal_target.image_filename)
         ideal_target.extract_features(feature_extractor)
         ideal_target.filter_keypoints_by_polygons()
         ideal_target.compute_reprojection_maps()
@@ -390,7 +410,9 @@
     AUTO_PROJECTION = True
 
     if AUTO_PROJECTION:
-        glog.info("\n\nAuto projection of training keypoints to 3D using ideal images")
+        glog.info(
+            "\n\nAuto projection of training keypoints to 3D using ideal images"
+        )
         # Match the captured training image against the "ideal" training image
         # and use those matches to pin down the 3D locations of the keypoints
 
@@ -399,7 +421,8 @@
             training_target = training_target_list[target_ind]
             ideal_target = ideal_target_list[target_ind]
 
-            glog.info("\nPreparing target for image %s" % training_target.image_filename)
+            glog.info("\nPreparing target for image %s" %
+                      training_target.image_filename)
             # Extract keypoints and descriptors for model
             training_target.extract_features(feature_extractor)
 
@@ -419,7 +442,8 @@
             H_inv = np.linalg.inv(homography_list[0])
 
             for polygon in ideal_target.polygon_list:
-                ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(-1, 1, 2)
+                ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(
+                    -1, 1, 2)
                 # We use the ideal target's polygons to define the polygons on
                 # the training target
                 transformed_polygon = cv2.perspectiveTransform(
@@ -427,10 +451,14 @@
                 training_target.polygon_list.append(transformed_polygon)
 
             # Also project the target point from the ideal to training image
-            training_target_point_2d = cv2.perspectiveTransform(np.asarray(ideal_target.target_point_2d).reshape(-1,1,2), H_inv)
-            training_target.target_point_2d = training_target_point_2d.reshape(-1,1,2)
+            training_target_point_2d = cv2.perspectiveTransform(
+                np.asarray(ideal_target.target_point_2d).reshape(-1, 1, 2),
+                H_inv)
+            training_target.target_point_2d = training_target_point_2d.reshape(
+                -1, 1, 2)
 
-            glog.info("Started with %d keypoints" % len(training_target.keypoint_list))
+            glog.info("Started with %d keypoints" % len(
+                training_target.keypoint_list))
 
             training_target.keypoint_list, training_target.descriptor_list, rejected_keypoint_list, rejected_descriptor_list, _ = dtd.filter_keypoints_by_polygons(
                 training_target.keypoint_list, training_target.descriptor_list,
@@ -490,8 +518,17 @@
 
 if __name__ == '__main__':
     ap = argparse.ArgumentParser()
-    ap.add_argument("--visualize", help="Whether to visualize the results", default=False, action='store_true')
-    ap.add_argument("-n", "--no_bazel", help="Don't run using Bazel", default=True, action='store_false')
+    ap.add_argument(
+        "--visualize",
+        help="Whether to visualize the results",
+        default=False,
+        action='store_true')
+    ap.add_argument(
+        "-n",
+        "--no_bazel",
+        help="Don't run using Bazel",
+        default=True,
+        action='store_false')
     args = vars(ap.parse_args())
 
     VISUALIZE_KEYPOINTS = args["visualize"]
diff --git a/y2020/vision/tools/python_code/target_definition_test.py b/y2020/vision/tools/python_code/target_definition_test.py
index 18df1e9..3c4d308 100644
--- a/y2020/vision/tools/python_code/target_definition_test.py
+++ b/y2020/vision/tools/python_code/target_definition_test.py
@@ -26,5 +26,6 @@
     target_data_test_1.target_rotation = np.identity(3, np.double)
     target_data_test_1.target_position = np.array([0., 1., 2.])
     target_data_test_1.target_point_2d = np.array([10., 20.]).reshape(-1, 1, 2)
+    target_data_test_1.target_radius = 32
     target_data_list.append(target_data_test_1)
     return target_data_list
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index 93c607c..f139e71 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -11,6 +11,7 @@
 
 glog.setLevel("WARN")
 
+
 # Calculates rotation matrix to euler angles
 # The result is the same as MATLAB except the order
 # of the euler angles ( x and z are swapped ).
@@ -175,13 +176,16 @@
     for i in range(len(train_keypoint_lists)):
         good_matches = good_matches_list[i]
         if len(good_matches) < MIN_MATCH_COUNT:
-            glog.warn("Not enough matches are for model %d: %d out of needed #: %d" % (i, len(good_matches), MIN_MATCH_COUNT))
+            glog.warn(
+                "Not enough matches are for model %d: %d out of needed #: %d" %
+                (i, len(good_matches), MIN_MATCH_COUNT))
             homography_list.append([])
             matches_mask_list.append([])
             continue
 
-        glog.info("Got good number of matches for model %d: %d (needed only %d)" %
-                  (i, len(good_matches), MIN_MATCH_COUNT))
+        glog.info(
+            "Got good number of matches for model %d: %d (needed only %d)" %
+            (i, len(good_matches), MIN_MATCH_COUNT))
         # Extract and bundle keypoint locations for computations
         src_pts = np.float32([
             train_keypoint_lists[i][m.trainIdx].pt for m in good_matches
@@ -216,8 +220,8 @@
         matches_mask_count = matches_mask_list[i].count(1)
         if matches_mask_count != len(good_matches):
             glog.info("Homography rejected some matches!  From ",
-                  len(good_matches), ", only ", matches_mask_count,
-                  " were used")
+                      len(good_matches), ", only ", matches_mask_count,
+                      " were used")
 
         if matches_mask_count < MIN_MATCH_COUNT:
             glog.info(
@@ -233,7 +237,8 @@
         query_box_pts = cv2.perspectiveTransform(train_box_pts, H)
 
         # Figure out where the training target goes on the query img
-        transformed_target = cv2.perspectiveTransform(target_point_list[i].reshape(-1,1,2), H)
+        transformed_target = cv2.perspectiveTransform(
+            target_point_list[i].reshape(-1, 1, 2), H)
         # Ballpark the size of the circle so it looks right on image
         radius = int(
             32 * abs(H[0][0] + H[1][1]) / 2)  # Average of scale factors
@@ -276,9 +281,11 @@
 #        keypoint_list: List of opencv keypoints
 def show_keypoints(image, keypoint_list):
     ret_img = image.copy()
-    ret_img = cv2.drawKeypoints(ret_img, keypoint_list, ret_img,
-                               flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+    ret_img = cv2.drawKeypoints(
+        ret_img,
+        keypoint_list,
+        ret_img,
+        flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
     cv2.imshow("Keypoints", ret_img)
     cv2.waitKey(0)
     return ret_img
-
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 292618b..e8c227d 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -16,6 +16,18 @@
     ],
 )
 
+ts_library(
+    name = "field_main",
+    srcs = [
+        "field_main.ts",
+        "field_handler.ts",
+        "constants.ts",
+    ],
+    deps = [
+        "//aos/network/www:proxy",
+    ],
+)
+
 rollup_bundle(
     name = "main_bundle",
     entry_point = "y2020/www/main",
@@ -25,6 +37,15 @@
     ],
 )
 
+rollup_bundle(
+    name = "field_main_bundle",
+    entry_point = "y2020/www/field_main",
+    deps = [
+        "field_main",
+    ],
+    visibility = ["//y2020:__subpackages__"],
+)
+
 filegroup(
     name = "files",
     srcs = glob([
diff --git a/y2020/www/constants.ts b/y2020/www/constants.ts
new file mode 100644
index 0000000..b94d7a7
--- /dev/null
+++ b/y2020/www/constants.ts
@@ -0,0 +1,7 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+export const FIELD_WIDTH = 26 * FT_TO_M + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 52 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2020/www/field.html b/y2020/www/field.html
new file mode 100644
index 0000000..37452a3
--- /dev/null
+++ b/y2020/www/field.html
@@ -0,0 +1,12 @@
+<html>
+  <head>
+    <script src="flatbuffers.js"></script>
+    <script src="field_main_bundle.min.js" defer></script>
+    <link rel="stylesheet" href="styles.css">
+  </head>
+  <body>
+    <div id="config">
+    </div>
+  </body>
+</html>
+
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
new file mode 100644
index 0000000..a960a63
--- /dev/null
+++ b/y2020/www/field_handler.ts
@@ -0,0 +1,167 @@
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_CENTER_X = (198.75 + 116) * IN_TO_M;
+
+const DS_WIDTH = 69 * IN_TO_M;
+const DS_ANGLE = 20 * Math.PI / 180;
+const DS_END_X = DS_WIDTH * Math.sin(DS_ANGLE);
+const OTHER_DS_X = FIELD_LENGTH - DS_END_X;
+const DS_INSIDE_Y = FIELD_SIDE_Y - DS_WIDTH * Math.cos(DS_ANGLE);
+
+const TRENCH_START_X = 206.57 * IN_TO_M;
+const TRENCH_END_X = FIELD_LENGTH - TRENCH_START_X;
+const TRENCH_WIDTH = 55.5 * IN_TO_M;
+const TRENCH_INSIDE = FIELD_SIDE_Y - TRENCH_WIDTH;
+
+const SPINNER_LENGTH = 30 * IN_TO_M;
+const SPINNER_TOP_X = 374.59 * IN_TO_M;
+const SPINNER_BOTTOM_X = SPINNER_TOP_X - SPINNER_LENGTH;
+
+const SHIELD_BOTTOM_X = FIELD_CENTER_X - 116 * IN_TO_M;
+const SHIELD_BOTTOM_Y = 43.75 * IN_TO_M;
+
+const SHIELD_TOP_X = FIELD_CENTER_X + 116 * IN_TO_M;
+const SHIELD_TOP_Y = -43.75 * IN_TO_M;
+
+const SHIELD_RIGHT_X = FIELD_CENTER_X - 51.06 * IN_TO_M;
+const SHIELD_RIGHT_Y = -112.88 * IN_TO_M;
+
+const SHIELD_LEFT_X = FIELD_CENTER_X + 51.06 * IN_TO_M;
+const SHIELD_LEFT_Y = 112.88 * IN_TO_M;
+
+const SHIELD_CENTER_TOP_X = (SHIELD_TOP_X + SHIELD_LEFT_X) / 2
+const SHIELD_CENTER_TOP_Y = (SHIELD_TOP_Y + SHIELD_LEFT_Y) / 2
+
+const SHIELD_CENTER_BOTTOM_X = (SHIELD_BOTTOM_X + SHIELD_RIGHT_X) / 2
+const SHIELD_CENTER_BOTTOM_Y = (SHIELD_BOTTOM_Y + SHIELD_RIGHT_Y) / 2
+
+const INITIATION_X = 120 * IN_TO_M;
+const FAR_INITIATION_X = FIELD_LENGTH - 120 * IN_TO_M;
+
+const TARGET_ZONE_TIP_X = 30 * IN_TO_M;
+const TARGET_ZONE_WIDTH = 48 * IN_TO_M;
+const LOADING_ZONE_WIDTH = 60 * IN_TO_M;
+
+export class FieldHandler {
+  private canvas = document.createElement('canvas');
+
+  constructor() {
+    document.body.appendChild(this.canvas);
+  }
+
+  drawField(): void {
+    const MY_COLOR = 'red';
+    const OTHER_COLOR = 'blue';
+    const ctx = this.canvas.getContext('2d');
+    // draw perimiter
+    ctx.beginPath();
+    ctx.moveTo(0, DS_INSIDE_Y);
+    ctx.lineTo(DS_END_X, FIELD_SIDE_Y);
+    ctx.lineTo(OTHER_DS_X, FIELD_SIDE_Y);
+    ctx.lineTo(FIELD_LENGTH, DS_INSIDE_Y);
+    ctx.lineTo(FIELD_LENGTH, -DS_INSIDE_Y);
+    ctx.lineTo(OTHER_DS_X, -FIELD_SIDE_Y);
+    ctx.lineTo(DS_END_X, -FIELD_SIDE_Y);
+    ctx.lineTo(0, -DS_INSIDE_Y);
+    ctx.lineTo(0, DS_INSIDE_Y);
+    ctx.stroke();
+
+    // draw shield generator
+    ctx.beginPath();
+    ctx.moveTo(SHIELD_BOTTOM_X, SHIELD_BOTTOM_Y);
+    ctx.lineTo(SHIELD_RIGHT_X, SHIELD_RIGHT_Y);
+    ctx.lineTo(SHIELD_TOP_X, SHIELD_TOP_Y);
+    ctx.lineTo(SHIELD_LEFT_X, SHIELD_LEFT_Y);
+    ctx.lineTo(SHIELD_BOTTOM_X, SHIELD_BOTTOM_Y);
+    ctx.moveTo(SHIELD_CENTER_TOP_X, SHIELD_CENTER_TOP_Y);
+    ctx.lineTo(SHIELD_CENTER_BOTTOM_X, SHIELD_CENTER_BOTTOM_Y);
+    ctx.stroke();
+
+    // draw trenches
+    ctx.strokeStyle = MY_COLOR;
+    ctx.beginPath();
+    ctx.moveTo(TRENCH_START_X, FIELD_SIDE_Y);
+    ctx.lineTo(TRENCH_START_X, TRENCH_INSIDE);
+    ctx.lineTo(TRENCH_END_X, TRENCH_INSIDE);
+    ctx.lineTo(TRENCH_END_X, FIELD_SIDE_Y);
+    ctx.stroke();
+
+    ctx.strokeStyle = OTHER_COLOR;
+    ctx.beginPath();
+    ctx.moveTo(TRENCH_START_X, -FIELD_SIDE_Y);
+    ctx.lineTo(TRENCH_START_X, -TRENCH_INSIDE);
+    ctx.lineTo(TRENCH_END_X, -TRENCH_INSIDE);
+    ctx.lineTo(TRENCH_END_X, -FIELD_SIDE_Y);
+    ctx.stroke();
+
+    ctx.strokeStyle = 'black';
+    ctx.beginPath();
+    ctx.moveTo(SPINNER_TOP_X, FIELD_SIDE_Y);
+    ctx.lineTo(SPINNER_TOP_X, TRENCH_INSIDE);
+    ctx.lineTo(SPINNER_BOTTOM_X, TRENCH_INSIDE);
+    ctx.lineTo(SPINNER_BOTTOM_X, FIELD_SIDE_Y);
+    ctx.moveTo(FIELD_LENGTH - SPINNER_TOP_X, -FIELD_SIDE_Y);
+    ctx.lineTo(FIELD_LENGTH - SPINNER_TOP_X, -TRENCH_INSIDE);
+    ctx.lineTo(FIELD_LENGTH - SPINNER_BOTTOM_X, -TRENCH_INSIDE);
+    ctx.lineTo(FIELD_LENGTH - SPINNER_BOTTOM_X, -FIELD_SIDE_Y);
+    ctx.stroke();
+
+    // draw initiation lines
+    ctx.beginPath();
+    ctx.moveTo(INITIATION_X, FIELD_SIDE_Y);
+    ctx.lineTo(INITIATION_X, -FIELD_SIDE_Y);
+    ctx.moveTo(FAR_INITIATION_X, FIELD_SIDE_Y);
+    ctx.lineTo(FAR_INITIATION_X, -FIELD_SIDE_Y);
+    ctx.stroke();
+
+    // draw target/loading zones
+    ctx.strokeStyle = MY_COLOR;
+    ctx.beginPath();
+    ctx.moveTo(0, DS_INSIDE_Y);
+    ctx.lineTo(TARGET_ZONE_TIP_X, DS_INSIDE_Y - 0.5 * TARGET_ZONE_WIDTH);
+    ctx.lineTo(0, DS_INSIDE_Y - TARGET_ZONE_WIDTH);
+
+    ctx.moveTo(FIELD_LENGTH, DS_INSIDE_Y);
+    ctx.lineTo(
+        FIELD_LENGTH - TARGET_ZONE_TIP_X,
+        DS_INSIDE_Y - 0.5 * LOADING_ZONE_WIDTH);
+    ctx.lineTo(FIELD_LENGTH, DS_INSIDE_Y - LOADING_ZONE_WIDTH);
+    ctx.stroke();
+
+    ctx.strokeStyle = OTHER_COLOR;
+    ctx.beginPath();
+    ctx.moveTo(0, -DS_INSIDE_Y);
+    ctx.lineTo(TARGET_ZONE_TIP_X, -(DS_INSIDE_Y - 0.5 * LOADING_ZONE_WIDTH));
+    ctx.lineTo(0, -(DS_INSIDE_Y - LOADING_ZONE_WIDTH));
+
+    ctx.moveTo(FIELD_LENGTH, -DS_INSIDE_Y);
+    ctx.lineTo(
+        FIELD_LENGTH - TARGET_ZONE_TIP_X,
+        -(DS_INSIDE_Y - 0.5 * TARGET_ZONE_WIDTH));
+    ctx.lineTo(FIELD_LENGTH, -(DS_INSIDE_Y - TARGET_ZONE_WIDTH));
+    ctx.stroke();
+  }
+
+  reset(): void {
+    const ctx = this.canvas.getContext('2d');
+    ctx.setTransform(1, 0, 0, 1, 0, 0);
+    const size = window.innerHeight * 0.9;
+    ctx.canvas.height = size;
+    ctx.canvas.width = size / 2 + 10;
+    ctx.clearRect(0, 0, size, size / 2 + 10);
+
+    // Translate to center of bottom of display.
+    ctx.translate(size / 4, size);
+    // Coordinate system is:
+    // x -> forward.
+    // y -> to the left.
+    ctx.rotate(-Math.PI / 2);
+    ctx.scale(1, -1);
+    ctx.translate(5, 0);
+
+    const M_TO_PX = (size - 10) / FIELD_LENGTH;
+    ctx.scale(M_TO_PX, M_TO_PX);
+    ctx.lineWidth = 1 / M_TO_PX;
+  }
+}
diff --git a/y2020/www/field_main.ts b/y2020/www/field_main.ts
new file mode 100644
index 0000000..adcaa27
--- /dev/null
+++ b/y2020/www/field_main.ts
@@ -0,0 +1,13 @@
+import {Connection} from 'aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler();
+
+fieldHandler.reset();
+fieldHandler.drawField();
+
diff --git a/y2020/www/main.ts b/y2020/www/main.ts
index d414eac..46a0e8a 100644
--- a/y2020/www/main.ts
+++ b/y2020/www/main.ts
@@ -1,9 +1,12 @@
 import {Connection} from 'aos/network/www/proxy';
 
 import {ImageHandler} from './image_handler';
+import {ConfigHandler} from 'aos/network/www/config_handler';
 
 const conn = new Connection();
 
+const configPrinter = new ConfigHandler(conn);
+
 conn.connect();
 
 const iHandler = new ImageHandler();
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 0e1a6ba..3d966b8 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -380,6 +380,42 @@
       "max_size": 2000000
     },
     {
+      "name": "/pi3/camera",
+      "type": "frc971.vision.CameraImage",
+      "source_node": "pi3",
+      "frequency": 25,
+      "max_size": 620000,
+      "num_senders": 18
+    },
+    {
+      "name": "/pi3/camera",
+      "type": "frc971.vision.sift.ImageMatchResult",
+      "source_node": "pi3",
+      "frequency": 25,
+      "max_size": 10000,
+      "destination_nodes": [
+        {
+          "name": "roborio",
+          "priority": 1,
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/pi3/camera/detailed",
+      "type": "frc971.vision.sift.ImageMatchResult",
+      "source_node": "pi3",
+      "frequency": 25,
+      "max_size": 300000
+    },
+    {
+      "name": "/pi3/camera",
+      "type": "frc971.vision.sift.TrainingData",
+      "source_node": "pi3",
+      "frequency": 2,
+      "max_size": 2000000
+    },
+    {
       "name": "/autonomous",
       "type": "aos.common.actions.Status",
       "source_node": "roborio"
@@ -491,6 +527,24 @@
     },
     {
       "match": {
+        "name": "/camera",
+        "source_node": "pi3"
+      },
+      "rename": {
+        "name": "/pi3/camera"
+      }
+    },
+    {
+      "match": {
+        "name": "/camera/detailed",
+        "source_node": "pi3"
+      },
+      "rename": {
+        "name": "/pi3/camera/detailed"
+      }
+    },
+    {
+      "match": {
         "name": "/aos",
         "type": "aos.RobotState"
       },