diff --git a/README.md b/README.md
index 99154c4..d54d963 100644
--- a/README.md
+++ b/README.md
@@ -128,3 +128,21 @@
 # log messages and changes.
    apt-get install gitg
 ```
+
+### Roborio Kernel Traces
+
+Currently (as of 2020.02.26), top tends to produce misleading statistics. As
+such, you can get more useful information about CPU usage by using kernel
+traces. Sample usage:
+```console
+# Note that you will need to install the trace-cmd package on the roborio.
+# This may be not be a trivial task.
+# Start the trace
+trace-cmd start -e sched_switch -e workqueue
+# Stop the trace
+trace-cmd stop
+# Save the trace to trace.dat
+trace-cmd extract
+```
+You can then scp the `trace.dat` file to your computer and run `kernelshark
+trace.dat` (may require installing the `kernelshark` apt package).
diff --git a/aos/BUILD b/aos/BUILD
index 12d25b2..1d6d388 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -477,6 +477,7 @@
         "testdata/config2_multinode.json",
         "testdata/config3.json",
         "testdata/expected.json",
+        "testdata/expected_merge_with.json",
         "testdata/expected_multinode.json",
         "testdata/good_multinode.json",
         "testdata/good_multinode_hostnames.json",
diff --git a/aos/configuration.cc b/aos/configuration.cc
index ac897bf..d6d276e 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -417,6 +417,14 @@
   return MergeConfiguration(ReadConfig(path, &visited_paths));
 }
 
+FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
+    const Configuration *config, std::string_view json) {
+  FlatbufferDetachedBuffer<Configuration> addition =
+      JsonToFlatbuffer(json, Configuration::MiniReflectTypeTable());
+
+  return MergeConfiguration(MergeFlatBuffers(config, &addition.message()));
+}
+
 const Channel *GetChannel(const Configuration *config, std::string_view name,
                           std::string_view type,
                           std::string_view application_name, const Node *node) {
diff --git a/aos/configuration.h b/aos/configuration.h
index bf2ec6c..9193fd5 100644
--- a/aos/configuration.h
+++ b/aos/configuration.h
@@ -32,6 +32,11 @@
     const Flatbuffer<Configuration> &config,
     const std::vector<aos::FlatbufferString<reflection::Schema>> &schemas);
 
+// Merges a configuration json snippet into the provided configuration and
+// returns the modified config.
+FlatbufferDetachedBuffer<Configuration> MergeWithConfig(
+    const Configuration *config, std::string_view json);
+
 // Returns the resolved location for a name, type, and application name. Returns
 // nullptr if none is found.
 //
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index c36bd93..f069277 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -13,6 +13,8 @@
 namespace configuration {
 namespace testing {
 
+const std::string kConfigPrefix = "aos/testdata/";
+
 class ConfigurationTest : public ::testing::Test {
  public:
   ConfigurationTest() { ::aos::testing::EnableTestLogging(); }
@@ -30,19 +32,19 @@
 // Tests that we can read and merge a configuration.
 TEST_F(ConfigurationTest, ConfigMerge) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/config1.json");
+      ReadConfig(kConfigPrefix + "config1.json");
   LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
 
   EXPECT_EQ(
       absl::StripSuffix(
-          util::ReadFileToStringOrDie("aos/testdata/expected.json"), "\n"),
+          util::ReadFileToStringOrDie(kConfigPrefix + "expected.json"), "\n"),
       FlatbufferToJson(config, true));
 }
 
 // Tests that we can get back a ChannelIndex.
 TEST_F(ConfigurationTest, ChannelIndex) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/config1.json");
+      ReadConfig(kConfigPrefix + "config1.json");
 
   EXPECT_EQ(
       ChannelIndex(&config.message(), config.message().channels()->Get(1u)),
@@ -52,12 +54,12 @@
 // Tests that we can read and merge a multinode configuration.
 TEST_F(ConfigurationTest, ConfigMergeMultinode) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/config1_multinode.json");
+      ReadConfig(kConfigPrefix + "config1_multinode.json");
   LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
 
   EXPECT_EQ(
       std::string(absl::StripSuffix(
-          util::ReadFileToStringOrDie("aos/testdata/expected_multinode.json"),
+          util::ReadFileToStringOrDie(kConfigPrefix + "expected_multinode.json"),
           "\n")),
       FlatbufferToJson(config, true));
 }
@@ -65,7 +67,7 @@
 // Tests that we sort the entries in a config so we can look entries up.
 TEST_F(ConfigurationTest, UnsortedConfig) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/backwards.json");
+      ReadConfig(kConfigPrefix + "backwards.json");
 
   LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
 
@@ -80,16 +82,41 @@
   EXPECT_DEATH(
       {
         FlatbufferDetachedBuffer<Configuration> config =
-            ReadConfig("aos/testdata/config1_bad.json");
+            ReadConfig(kConfigPrefix + "config1_bad.json");
       },
-      "aos/testdata/config1_bad.json");
+      kConfigPrefix + "config1_bad.json");
+}
+
+// Tests that we can modify a config with a json snippet.
+TEST_F(ConfigurationTest, MergeWithConfig) {
+  FlatbufferDetachedBuffer<Configuration> config =
+      ReadConfig(kConfigPrefix + "config1.json");
+  LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
+
+  FlatbufferDetachedBuffer<Configuration> updated_config =
+      MergeWithConfig(&config.message(),
+                      R"channel({
+  "channels": [
+    {
+      "name": "/foo",
+      "type": ".aos.bar",
+      "max_size": 100
+    }
+  ]
+})channel");
+
+  EXPECT_EQ(
+      absl::StripSuffix(util::ReadFileToStringOrDie(
+                            kConfigPrefix + "expected_merge_with.json"),
+                        "\n"),
+      FlatbufferToJson(updated_config, true));
 }
 
 // Tests that we can lookup a location, complete with maps, from a merged
 // config.
 TEST_F(ConfigurationTest, GetChannel) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/config1.json");
+      ReadConfig(kConfigPrefix + "config1.json");
 
   // Test a basic lookup first.
   EXPECT_EQ(
@@ -123,7 +150,7 @@
 // Tests that we can lookup a location with node specific maps.
 TEST_F(ConfigurationTest, GetChannelMultinode) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   const Node *pi1 = GetNode(&config.message(), "pi1");
   const Node *pi2 = GetNode(&config.message(), "pi2");
 
@@ -159,7 +186,7 @@
 // Tests that we can lookup a location with type specific maps.
 TEST_F(ConfigurationTest, GetChannelTypedMultinode) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   const Node *pi1 = GetNode(&config.message(), "pi1");
 
   // Test a basic lookup first.
@@ -182,28 +209,28 @@
   EXPECT_DEATH(
       {
         FlatbufferDetachedBuffer<Configuration> config =
-            ReadConfig("aos/testdata/invalid_nodes.json");
+            ReadConfig(kConfigPrefix + "invalid_nodes.json");
       },
       "source_node");
 
   EXPECT_DEATH(
       {
         FlatbufferDetachedBuffer<Configuration> config =
-            ReadConfig("aos/testdata/invalid_source_node.json");
+            ReadConfig(kConfigPrefix + "invalid_source_node.json");
       },
       "source_node");
 
   EXPECT_DEATH(
       {
         FlatbufferDetachedBuffer<Configuration> config =
-            ReadConfig("aos/testdata/invalid_destination_node.json");
+            ReadConfig(kConfigPrefix + "invalid_destination_node.json");
       },
       "destination_nodes");
 
   EXPECT_DEATH(
       {
         FlatbufferDetachedBuffer<Configuration> config =
-            ReadConfig("aos/testdata/self_forward.json");
+            ReadConfig(kConfigPrefix + "self_forward.json");
       },
       "forwarding data to itself");
 }
@@ -582,7 +609,7 @@
 // Tests that we can deduce source nodes from a multinode config.
 TEST_F(ConfigurationTest, SourceNodeNames) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/config1_multinode.json");
+      ReadConfig(kConfigPrefix + "config1_multinode.json");
 
   // This is a bit simplistic in that it doesn't test deduplication, but it does
   // exercise a lot of the logic.
@@ -597,7 +624,7 @@
 // Tests that we can deduce destination nodes from a multinode config.
 TEST_F(ConfigurationTest, DestinationNodeNames) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/config1_multinode.json");
+      ReadConfig(kConfigPrefix + "config1_multinode.json");
 
   // This is a bit simplistic in that it doesn't test deduplication, but it does
   // exercise a lot of the logic.
@@ -613,7 +640,7 @@
 TEST_F(ConfigurationTest, GetNodes) {
   {
     FlatbufferDetachedBuffer<Configuration> config =
-        ReadConfig("aos/testdata/good_multinode.json");
+        ReadConfig(kConfigPrefix + "good_multinode.json");
     const Node *pi1 = GetNode(&config.message(), "pi1");
     const Node *pi2 = GetNode(&config.message(), "pi2");
 
@@ -622,7 +649,7 @@
 
   {
     FlatbufferDetachedBuffer<Configuration> config =
-        ReadConfig("aos/testdata/config1.json");
+        ReadConfig(kConfigPrefix + "config1.json");
     EXPECT_THAT(GetNodes(&config.message()), ::testing::ElementsAre(nullptr));
   }
 }
@@ -630,9 +657,9 @@
 // Tests that we can extract a node index from a config.
 TEST_F(ConfigurationTest, GetNodeIndex) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   FlatbufferDetachedBuffer<Configuration> config2 =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   const Node *pi1 = GetNode(&config.message(), "pi1");
   const Node *pi2 = GetNode(&config.message(), "pi2");
 
@@ -653,13 +680,13 @@
 // valid nodes.
 TEST_F(ConfigurationDeathTest, GetNodeOrDie) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   FlatbufferDetachedBuffer<Configuration> config2 =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   {
     // Simple case, nullptr -> nullptr
     FlatbufferDetachedBuffer<Configuration> single_node_config =
-        ReadConfig("aos/testdata/config1.json");
+        ReadConfig(kConfigPrefix + "config1.json");
     EXPECT_EQ(nullptr, GetNodeOrDie(&single_node_config.message(), nullptr));
 
     // Confirm that we die when a node is passed in.
@@ -679,7 +706,7 @@
 
 TEST_F(ConfigurationTest, GetNodeFromHostname) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/good_multinode.json");
+      ReadConfig(kConfigPrefix + "good_multinode.json");
   EXPECT_EQ("pi1",
             CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
                 ->name()
@@ -695,7 +722,7 @@
 
 TEST_F(ConfigurationTest, GetNodeFromHostnames) {
   FlatbufferDetachedBuffer<Configuration> config =
-      ReadConfig("aos/testdata/good_multinode_hostnames.json");
+      ReadConfig(kConfigPrefix + "good_multinode_hostnames.json");
   EXPECT_EQ("pi1",
             CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
                 ->name()
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index 7ad5683..536df02 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -114,6 +114,10 @@
     return &configuration_.message();
   }
 
+  SimulatedEventLoopFactory *event_loop_factory() {
+    return &event_loop_factory_;
+  }
+
  private:
   // Sends out all of the required queue messages.
   void SendJoystickState() {
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index 02b7bd7..ff263f8 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -37,7 +37,7 @@
       PCHECK(rename(FLAGS_logfile.c_str(), orig_path.c_str()) == 0);
 
       aos::logger::SpanReader span_reader(orig_path);
-      CHECK(span_reader.ReadMessage().empty());
+      CHECK(!span_reader.ReadMessage().empty()) << ": Empty header, aborting";
 
       aos::logger::DetachedBufferWriter buffer_writer(FLAGS_logfile);
       buffer_writer.QueueSizedFlatbuffer(&fbb);
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index dad62cd..57bcb1c 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -58,7 +58,7 @@
 }
 std::string ShmPath(const Channel *channel) {
   CHECK(channel->has_type());
-  return ShmFolder(channel) + channel->type()->str() + ".v1";
+  return ShmFolder(channel) + channel->type()->str() + ".v2";
 }
 
 class MMapedQueue {
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 87411d2..0c39704 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -27,12 +27,12 @@
     }
 
     // Clean up anything left there before.
-    unlink((FLAGS_shm_base + "/test/aos.TestMessage.v1").c_str());
-    unlink((FLAGS_shm_base + "/test1/aos.TestMessage.v1").c_str());
-    unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v1").c_str());
-    unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v1").c_str());
-    unlink((FLAGS_shm_base + "/aos/aos.timing.Report.v1").c_str());
-    unlink((FLAGS_shm_base + "/aos/aos.logging.LogMessageFbs.v1").c_str());
+    unlink((FLAGS_shm_base + "/test/aos.TestMessage.v2").c_str());
+    unlink((FLAGS_shm_base + "/test1/aos.TestMessage.v2").c_str());
+    unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v2").c_str());
+    unlink((FLAGS_shm_base + "/test2/aos.TestMessage.v2").c_str());
+    unlink((FLAGS_shm_base + "/aos/aos.timing.Report.v2").c_str());
+    unlink((FLAGS_shm_base + "/aos/aos.logging.LogMessageFbs.v2").c_str());
   }
 
   ~ShmEventLoopTestFactory() { FLAGS_override_hostname = ""; }
diff --git a/aos/ipc_lib/lockless_queue.cc b/aos/ipc_lib/lockless_queue.cc
index c323b8b..7126ffd 100644
--- a/aos/ipc_lib/lockless_queue.cc
+++ b/aos/ipc_lib/lockless_queue.cc
@@ -306,6 +306,7 @@
     }
 
     memory->next_queue_index.Invalidate();
+    memory->uid = getuid();
 
     for (size_t i = 0; i < memory->num_senders(); ++i) {
       ::aos::ipc_lib::Sender *s = memory->GetSender(i);
@@ -319,6 +320,8 @@
     // Signal everything is done.  This needs to be done last, so if we die, we
     // redo initialization.
     memory->initialized = true;
+  } else {
+    CHECK_EQ(getuid(), memory->uid) << ": UIDs must match for all processes";
   }
 
   return memory;
@@ -871,6 +874,8 @@
               << memory->next_queue_index.Load(queue_size).DebugString()
               << ::std::endl;
 
+  ::std::cout << "    uid_t uid = " << memory->uid << ::std::endl;
+
   ::std::cout << "  }" << ::std::endl;
   ::std::cout << "  AtomicIndex queue[" << queue_size << "] {" << ::std::endl;
   for (size_t i = 0; i < queue_size; ++i) {
diff --git a/aos/ipc_lib/lockless_queue_memory.h b/aos/ipc_lib/lockless_queue_memory.h
index cbe76a7..a10609c 100644
--- a/aos/ipc_lib/lockless_queue_memory.h
+++ b/aos/ipc_lib/lockless_queue_memory.h
@@ -50,6 +50,11 @@
   // A message is valid iff its internal index matches the index in the queue.
   AtomicQueueIndex next_queue_index;
 
+  // All processes using a queue need to be able to send messages to each other.
+  // We're going to require they have matching UIDs, which is sufficient to do
+  // that.
+  uid_t uid;
+
   // There is then memory allocated after this structure.  That memory is used
   // to store the messages, queue, watchers, and senders.  This is equivalent to
   // writing:
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index 214df63..68a493b 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -156,6 +156,6 @@
 flatbuffer_cc_library(
     name = "log_message_fbs",
     srcs = ["log_message.fbs"],
-    visibility = ["//visibility:public"],
     gen_reflections = 1,
+    visibility = ["//visibility:public"],
 )
diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index 83a39c8..f025054 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -144,21 +144,25 @@
 
   char *tmp;
   AllocateLogName(&tmp, folder, basename);
+
+  std::string log_base_name = tmp;
+  std::string log_roborio_name = log_base_name + "_roborio_data.bfbs";
+  free(tmp);
+
   char *tmp2;
-  if (asprintf(&tmp2, "%s/%s-current", folder, basename) == -1) {
+  if (asprintf(&tmp2, "%s/%s-current.bfbs", folder, basename) == -1) {
     PLOG(WARNING) << "couldn't create current symlink name";
   } else {
     if (unlink(tmp2) == -1 && (errno != EROFS && errno != ENOENT)) {
       LOG(WARNING) << "unlink('" << tmp2 << "') failed";
     }
-    if (symlink(tmp, tmp2) == -1) {
-      PLOG(WARNING) << "symlink('" << tmp << "', '" << tmp2 << "') failed";
+    if (symlink(log_roborio_name.c_str(), tmp2) == -1) {
+      PLOG(WARNING) << "symlink('" << log_roborio_name.c_str() << "', '" << tmp2
+                    << "') failed";
     }
     free(tmp2);
   }
-  std::string result = tmp;
-  free(tmp);
-  return result;
+  return log_base_name;
 }
 
 }  // namespace logging
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 89853f0..8f3f394 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -70,6 +70,7 @@
     ],
     deps = [
         ":team_number",
+        "//aos:configuration",
         "//aos/testing:googletest",
     ],
 )
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index 14fadab..d2fbc8e 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -9,6 +9,8 @@
 
 #include "aos/util/string_to_num.h"
 
+DECLARE_string(override_hostname);
+
 namespace aos {
 namespace network {
 namespace team_number_internal {
@@ -101,10 +103,14 @@
 }  // namespace
 
 ::std::string GetHostname() {
-  char buf[256];
-  buf[sizeof(buf) - 1] = '\0';
-  PCHECK(gethostname(buf, sizeof(buf) - 1) == 0);
-  return buf;
+  if (FLAGS_override_hostname.empty()) {
+    char buf[256];
+    buf[sizeof(buf) - 1] = '\0';
+    PCHECK(gethostname(buf, sizeof(buf) - 1) == 0);
+    return buf;
+  } else {
+    return FLAGS_override_hostname;
+  }
 }
 
 uint16_t GetTeamNumber() {
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index f4a8ce8..5b50072 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -53,6 +53,8 @@
   }
 
   fetcher_->Fetch();
+  VLOG(2) << "Sending a message with " << GetPacketCount(fetcher_->context())
+          << "packets";
   for (int packet_index = 0; packet_index < GetPacketCount(fetcher_->context());
        ++packet_index) {
     flatbuffers::Offset<MessageHeader> message =
@@ -65,14 +67,20 @@
         rtc::CopyOnWriteBuffer(buffer.data(), buffer.size()),
         true /* binary array */);
     for (auto conn : channels_) {
+      if (conn->buffered_amount() > 14000000) {
+        VLOG(1) << "skipping a send because buffered amount is too high";
+        continue;
+      }
       conn->Send(data_buffer);
     }
   }
 }
 
 bool Subscriber::Compare(const Channel *channel) const {
-  return channel->name() == fetcher_->channel()->name() &&
-         channel->type() == fetcher_->channel()->type();
+  return channel->name()->string_view() ==
+             fetcher_->channel()->name()->string_view() &&
+         channel->type()->string_view() ==
+             fetcher_->channel()->type()->string_view();
 }
 
 Connection::Connection(
@@ -156,6 +164,7 @@
   webrtc::DataBuffer data_buffer(
       rtc::CopyOnWriteBuffer(buffer.data(), buffer.size()),
       true /* binary array */);
+  VLOG(1) << "Sending " << buffer.size() << "bytes to a client";
   data_channel_->Send(data_buffer);
 }
 
@@ -211,9 +220,11 @@
 void Connection::OnMessage(const webrtc::DataBuffer &buffer) {
   const message_bridge::Connect *message =
       flatbuffers::GetRoot<message_bridge::Connect>(buffer.data.data());
+  VLOG(2) << "Got a connect message " << aos::FlatbufferToJson(message);
   for (auto &subscriber : subscribers_) {
     // Make sure the subscriber is for a channel on this node.
     if (subscriber.get() == nullptr) {
+      VLOG(2) << ": Null subscriber";
       continue;
     }
     bool found_match = false;
diff --git a/aos/network/web_proxy_main.cc b/aos/network/web_proxy_main.cc
index 479320a..c674520 100644
--- a/aos/network/web_proxy_main.cc
+++ b/aos/network/web_proxy_main.cc
@@ -1,4 +1,5 @@
 #include "aos/events/shm_event_loop.h"
+#include "aos/flatbuffer_merge.h"
 #include "aos/init.h"
 #include "aos/network/web_proxy.h"
 #include "aos/seasocks/seasocks_logger.h"
@@ -15,17 +16,22 @@
     std::vector<std::unique_ptr<aos::web_proxy::Subscriber>> *subscribers,
     const aos::FlatbufferDetachedBuffer<aos::Configuration> &config) {
   aos::ShmEventLoop event_loop(&config.message());
-  const aos::Node *self = aos::configuration::GetMyNode(&config.message());
+  const bool is_multi_node =
+      aos::configuration::MultiNode(event_loop.configuration());
+  const aos::Node *self =
+      is_multi_node ? aos::configuration::GetMyNode(event_loop.configuration())
+                    : nullptr;
 
-  // TODO(alex): skip fetchers on the wrong node.
+  LOG(INFO) << "My node is " << aos::FlatbufferToJson(self);
+
   for (uint i = 0; i < config.message().channels()->size(); ++i) {
     auto channel = config.message().channels()->Get(i);
-    if (!aos::configuration::ChannelIsReadableOnNode(channel, self)) {
-      subscribers->emplace_back(nullptr);
-    } else {
+    if (aos::configuration::ChannelIsReadableOnNode(channel, self)) {
       auto fetcher = event_loop.MakeRawFetcher(channel);
       subscribers->emplace_back(
           std::make_unique<aos::web_proxy::Subscriber>(std::move(fetcher), i));
+    } else {
+      subscribers->emplace_back(nullptr);
     }
   }
 
@@ -33,7 +39,9 @@
 
   auto timer = event_loop.AddTimer([&]() {
     for (auto &subscriber : *subscribers) {
-      subscriber->RunIteration();
+      if (subscriber != nullptr) {
+        subscriber->RunIteration();
+      }
     }
   });
 
@@ -54,6 +62,14 @@
   aos::FlatbufferDetachedBuffer<aos::Configuration> config =
       aos::configuration::ReadConfig(FLAGS_config);
 
+  for (size_t i = 0; i < config.message().channels()->size(); ++i) {
+    aos::Channel *channel =
+        config.mutable_message()->mutable_channels()->GetMutableObject(i);
+    channel->clear_schema();
+  }
+
+  config = aos::CopyFlatBuffer(&config.message());
+
   std::vector<std::unique_ptr<aos::web_proxy::Subscriber>> subscribers;
 
   std::thread data_thread{
diff --git a/aos/network/www/BUILD b/aos/network/www/BUILD
index bab5198..67e54f8 100644
--- a/aos/network/www/BUILD
+++ b/aos/network/www/BUILD
@@ -7,7 +7,7 @@
         "**/*.html",
         "**/*.css",
     ]),
-    visibility=["//visibility:public"],
+    visibility = ["//visibility:public"],
 )
 
 ts_library(
@@ -16,12 +16,12 @@
         "config_handler.ts",
         "proxy.ts",
     ],
+    visibility = ["//visibility:public"],
     deps = [
-        "//aos/network:web_proxy_ts_fbs",
-        "//aos/network:connect_ts_fbs",
         "//aos:configuration_ts_fbs",
+        "//aos/network:connect_ts_fbs",
+        "//aos/network:web_proxy_ts_fbs",
     ],
-    visibility=["//visibility:public"],
 )
 
 ts_library(
@@ -39,10 +39,10 @@
 rollup_bundle(
     name = "main_bundle",
     entry_point = "aos/network/www/main",
+    visibility = ["//aos:__subpackages__"],
     deps = [
         "main",
     ],
-    visibility=["//aos:__subpackages__"],
 )
 
 genrule(
@@ -54,5 +54,5 @@
         "flatbuffers.js",
     ],
     cmd = "cp $(location @com_github_google_flatbuffers//:flatjs) $@",
-    visibility=["//aos:__subpackages__"],
+    visibility = ["//aos:__subpackages__"],
 )
diff --git a/aos/network/www/config_handler.ts b/aos/network/www/config_handler.ts
index 72b496e..6615f39 100644
--- a/aos/network/www/config_handler.ts
+++ b/aos/network/www/config_handler.ts
@@ -1,18 +1,36 @@
 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 root_div = document.createElement('div');
+  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));
+
+    document.body.appendChild(this.root_div);
+    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 +78,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 80c71b2..7bcf575 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -1,5 +1,6 @@
 import {ConfigHandler} from './config_handler';
 import {Configuration} from 'aos/configuration_generated';
+import * as WebProxy from 'aos/network/web_proxy_generated';
 
 // There is one handler for each DataChannel, it maintains the state of
 // multi-part messages and delegates to a callback when the message is fully
@@ -18,13 +19,17 @@
     const messageHeader =
         WebProxy.MessageHeader.getRootAsMessageHeader(fbBuffer);
     // Short circuit if only one packet
-    if (messageHeader.packetCount === 1) {
+    if (messageHeader.packetCount() === 1) {
       this.handlerFunc(messageHeader.dataArray());
       return;
     }
 
     if (messageHeader.packetIndex() === 0) {
       this.dataBuffer = new Uint8Array(messageHeader.length());
+      this.receivedMessageLength = 0;
+    }
+    if (!messageHeader.dataLength()) {
+      return;
     }
     this.dataBuffer.set(
         messageHeader.dataArray(),
@@ -44,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>();
 
@@ -54,6 +62,15 @@
     this.webSocketUrl = `ws://${server}/ws`;
   }
 
+  addConfigHandler(handler: (config: Configuration) => void): void {
+    this.configHandlers.add(handler);
+  }
+
+  /**
+   * Add a handler for a specific message type. Until we need to handle
+   * different channel names with the same type differently, this is good
+   * enough.
+   */
   addHandler(id: string, handler: (data: Uint8Array) => void): void {
     this.handlerFuncs.set(id, handler);
   }
@@ -67,17 +84,17 @@
         'message', (e) => this.onWebSocketMessage(e));
   }
 
+  getConfig() {
+    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 = aos.Configuration.getRootAsConfiguration(fbBuffer);
-      this.config = config;
-      this.configHandler = new ConfigHandler(config, this.dataChannel);
-      this.configHandler.printConfig();
-      return;
+    this.configInternal = Configuration.getRootAsConfiguration(fbBuffer);
+    for (const handler of Array.from(this.configHandlers)) {
+      handler(this.configInternal);
     }
   }
 
@@ -127,7 +144,7 @@
   onWebSocketOpen(): void {
     this.rtcPeerConnection = new RTCPeerConnection({});
     this.rtcPeerConnection.addEventListener(
-        'datachannel', (e) => this.onDataCnannel(e));
+        'datachannel', (e) => this.onDataChannel(e));
     this.dataChannel = this.rtcPeerConnection.createDataChannel('signalling');
     this.dataChannel.addEventListener(
         'message', (e) => this.onDataChannelMessage(e));
@@ -169,4 +186,14 @@
         break;
     }
   }
+
+  /**
+   * Subscribes to messages. Only the most recent connect message is in use. Any
+   * channels not specified in the message are implicitely unsubscribed.
+   * @param a Finished flatbuffer.Builder containing a Connect message to send.
+   */
+  sendConnectMessage(builder: any) {
+    const array = builder.asUint8Array();
+    this.dataChannel.send(array.buffer.slice(array.byteOffset));
+  }
 }
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index f5b14a5..e140351 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -6,6 +6,9 @@
 
   ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
   ln -s /var/local/natinst/log/FRC_UserProgram.log "${ROBOT_CODE}/FRC_UserProgram.log"
+elif [[ "$(hostname)" == "pi-"* ]]; then
+  ROBOT_CODE="/home/pi/robot_code"
+
 else
   ROBOT_CODE="${HOME}/robot_code"
 fi
diff --git a/aos/testdata/expected_merge_with.json b/aos/testdata/expected_merge_with.json
new file mode 100644
index 0000000..0ce2056
--- /dev/null
+++ b/aos/testdata/expected_merge_with.json
@@ -0,0 +1,64 @@
+{
+ "channels": [
+  {
+   "name": "/foo",
+   "type": ".aos.bar",
+   "max_size": 100
+  },
+  {
+   "name": "/foo2",
+   "type": ".aos.bar"
+  },
+  {
+   "name": "/foo3",
+   "type": ".aos.bar",
+   "max_size": 9
+  }
+ ],
+ "maps": [
+  {
+   "match": {
+    "name": "/batman"
+   },
+   "rename": {
+    "name": "/bar"
+   }
+  },
+  {
+   "match": {
+    "name": "/batman"
+   },
+   "rename": {
+    "name": "/foo"
+   }
+  }
+ ],
+ "applications": [
+  {
+   "name": "app1",
+   "maps": [
+    {
+     "match": {
+      "name": "/bar"
+     },
+     "rename": {
+      "name": "/foo"
+     }
+    }
+   ]
+  },
+  {
+   "name": "app2",
+   "maps": [
+    {
+     "match": {
+      "name": "/baz"
+     },
+     "rename": {
+      "name": "/foo"
+     }
+    }
+   ]
+  }
+ ]
+}
diff --git a/aos/testing/BUILD b/aos/testing/BUILD
index 9aa058d..8693f27 100644
--- a/aos/testing/BUILD
+++ b/aos/testing/BUILD
@@ -24,9 +24,9 @@
     visibility = ["//visibility:public"],
     deps = [
         ":googletest",
-        "@com_google_absl//absl/base",
         "//aos/logging:implementations",
         "//aos/mutex",
+        "@com_google_absl//absl/base",
     ],
 )
 
diff --git a/debian/webrtc.BUILD b/debian/webrtc.BUILD
index e90e268..23e029d 100644
--- a/debian/webrtc.BUILD
+++ b/debian/webrtc.BUILD
@@ -2,17 +2,17 @@
 
 cc_library(
     name = "webrtc",
-    visibility = ["//visibility:public"],
-    hdrs = glob(["include/**/*.h"]),
     srcs = cpu_select({
         "arm": ["lib/arm/Release/libwebrtc_full.a"],
         "else": ["lib/x64/Release/libwebrtc_full.a"],
     }),
+    hdrs = glob(["include/**/*.h"]),
     includes = ["include"],
+    visibility = ["//visibility:public"],
     deps = [
+        "@com_google_absl//absl/algorithm:container",
         "@com_google_absl//absl/strings",
         "@com_google_absl//absl/types:optional",
         "@com_google_absl//absl/types:variant",
-        "@com_google_absl//absl/algorithm:container",
     ],
 )
diff --git a/frc971/analysis/plot_configs/localizer.pb b/frc971/analysis/plot_configs/localizer.pb
index f81ec89..930d63d 100644
--- a/frc971/analysis/plot_configs/localizer.pb
+++ b/frc971/analysis/plot_configs/localizer.pb
@@ -23,6 +23,16 @@
   axes {
     line {
       y_signal {
+        channel: "DrivetrainStatus"
+        field: "trajectory_logging.y"
+      }
+      x_signal {
+        channel: "DrivetrainStatus"
+        field: "trajectory_logging.x"
+      }
+    }
+    line {
+      y_signal {
         channel: "DrivetrainTruthStatus"
         field: "y"
       }
@@ -31,7 +41,6 @@
         field: "x"
       }
     }
-    share_x_axis: false
     line {
       y_signal {
         channel: "DrivetrainStatus"
@@ -176,6 +185,18 @@
     line {
       y_signal {
         channel: "DrivetrainStatus"
+        field: "trajectory_logging.left_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "DrivetrainStatus"
+        field: "trajectory_logging.right_velocity"
+      }
+    }
+    line {
+      y_signal {
+        channel: "DrivetrainStatus"
         field: "localizer.left_velocity"
       }
     }
diff --git a/frc971/analysis/py_log_reader.cc b/frc971/analysis/py_log_reader.cc
index a9a1db1..08f3707 100644
--- a/frc971/analysis/py_log_reader.cc
+++ b/frc971/analysis/py_log_reader.cc
@@ -87,8 +87,14 @@
   tools->reader = std::make_unique<aos::logger::LogReader>(log_file_name);
   tools->reader->Register();
 
-  tools->event_loop =
-      tools->reader->event_loop_factory()->MakeEventLoop("data_fetcher");
+  if (aos::configuration::MultiNode(tools->reader->configuration())) {
+    tools->event_loop = tools->reader->event_loop_factory()->MakeEventLoop(
+        "data_fetcher",
+        aos::configuration::GetNode(tools->reader->configuration(), "roborio"));
+  } else {
+    tools->event_loop =
+        tools->reader->event_loop_factory()->MakeEventLoop("data_fetcher");
+  }
   tools->event_loop->SkipTimingReport();
   tools->event_loop->SkipAosLog();
 
diff --git a/frc971/constants.h b/frc971/constants.h
index debfb55..7c2121d 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -97,9 +97,9 @@
   double lower;
   double upper;
 
-  double middle() const { return (lower_hard + upper_hard) / 2.0; }
+  constexpr double middle() const { return (lower_hard + upper_hard) / 2.0; }
 
-  double range() const { return upper_hard - lower_hard; }
+  constexpr double range() const { return upper_hard - lower_hard; }
 };
 
 }  // namespace constants
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 9fef8f9..d223d13 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -562,7 +562,7 @@
         ":trajectory",
         "//aos/logging:implementations",
         "//aos/network:team_number",
-        "//y2019/control_loops/drivetrain:drivetrain_base",
+        "//y2020/control_loops/drivetrain:drivetrain_base",
         "@org_tuxfamily_eigen//:eigen",
     ],
 )
@@ -691,6 +691,7 @@
         "improved_down_estimator.h",
     ],
     deps = [
+        ":drivetrain_config",
         ":drivetrain_status_fbs",
         "//aos/events:event_loop",
         "//frc971/control_loops:control_loops_fbs",
@@ -707,6 +708,7 @@
         "improved_down_estimator_test.cc",
     ],
     deps = [
+        ":drivetrain_test_lib",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
         "//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a19f57b..1c0ac4f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,6 +41,7 @@
       gyro_reading_fetcher_(
           event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
               "/drivetrain")),
+      down_estimator_(dt_config),
       localizer_(localizer),
       kf_(dt_config_.make_kf_drivetrain_loop()),
       dt_openloop_(dt_config_, &kf_),
@@ -148,7 +149,7 @@
   }
 
   while (imu_values_fetcher_.FetchNext()) {
-    imu_zeroer_.ProcessMeasurement(*imu_values_fetcher_);
+    imu_zeroer_.InsertMeasurement(*imu_values_fetcher_);
     last_gyro_time_ = monotonic_now;
     if (!imu_zeroer_.Zeroed()) {
       continue;
@@ -165,6 +166,7 @@
 
   bool got_imu_reading = false;
   if (imu_values_fetcher_.get() != nullptr) {
+    imu_zeroer_.ProcessMeasurements();
     got_imu_reading = true;
     switch (dt_config_.imu_type) {
       case IMUType::IMU_X:
@@ -176,6 +178,9 @@
       case IMUType::IMU_Y:
         last_accel_ = -imu_values_fetcher_->accelerometer_y();
         break;
+      case IMUType::IMU_Z:
+        last_accel_ = imu_values_fetcher_->accelerometer_z();
+        break;
     }
   }
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 5a29008..a33ff22 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -37,6 +37,7 @@
   IMU_X = 0,          // Use the x-axis of the IMU.
   IMU_Y = 1,          // Use the y-axis of the IMU.
   IMU_FLIPPED_X = 2,  // Use the flipped x-axis of the IMU.
+  IMU_Z = 3,          // Use the z-axis of the IMU.
 };
 
 template <typename Scalar = double>
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 336025a..6a90122 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -190,16 +190,23 @@
   auto builder = imu_sender_.MakeBuilder();
   frc971::IMUValues::Builder imu_builder =
       builder.MakeBuilder<frc971::IMUValues>();
-  imu_builder.add_gyro_x(0.0);
-  imu_builder.add_gyro_y(0.0);
-  imu_builder.add_gyro_z(
-      (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
-      (dt_config_.robot_radius * 2.0));
+  const Eigen::Vector3d gyro =
+      dt_config_.imu_transform.inverse() *
+      Eigen::Vector3d(0.0, 0.0,
+                      (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+                          (dt_config_.robot_radius * 2.0));
+  imu_builder.add_gyro_x(gyro.x());
+  imu_builder.add_gyro_y(gyro.y());
+  imu_builder.add_gyro_z(gyro.z());
   // Acceleration due to gravity, in m/s/s.
   constexpr double kG = 9.807;
-  imu_builder.add_accelerometer_x(last_acceleration_.x() / kG);
-  imu_builder.add_accelerometer_y(last_acceleration_.y() / kG);
-  imu_builder.add_accelerometer_z(1.0);
+  const Eigen::Vector3d accel =
+      dt_config_.imu_transform.inverse() *
+      Eigen::Vector3d(last_acceleration_.x() / kG, last_acceleration_.y() / kG,
+                      1.0);
+  imu_builder.add_accelerometer_x(accel.x());
+  imu_builder.add_accelerometer_y(accel.y());
+  imu_builder.add_accelerometer_z(accel.z());
   imu_builder.add_monotonic_timestamp_ns(
       std::chrono::duration_cast<std::chrono::nanoseconds>(
           event_loop_->monotonic_now().time_since_epoch())
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index d2c0be3..cb50165 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -146,7 +146,7 @@
   static constexpr int kNOutputs = 3;
   // Time constant to use for estimating how the longitudinal/lateral velocity
   // offsets decay, in seconds.
-  static constexpr double kVelocityOffsetTimeConstant = 10.0;
+  static constexpr double kVelocityOffsetTimeConstant = 1.0;
   static constexpr double kLateralVelocityTimeConstant = 1.0;
   // Inputs are [left_volts, right_volts]
   typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 27de867..c1c8e1b 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -6,6 +6,7 @@
 
 #include "aos/events/event_loop.h"
 #include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/runge_kutta.h"
 #include "glog/logging.h"
@@ -276,6 +277,8 @@
 // accelerometer calibration).
 class DrivetrainUkf : public QuaternionUkf {
  public:
+  DrivetrainUkf(const DrivetrainConfig<double> &dt_config)
+      : QuaternionUkf(dt_config.imu_transform) {}
   // UKF for http://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf
   // Reference in case the link is dead:
   // Kraft, Edgar. "A quaternion-based unscented Kalman filter for orientation
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 86ef5e9..2edce5a 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -4,6 +4,7 @@
 #include <random>
 
 #include "aos/testing/random_seed.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
 #include "frc971/control_loops/drivetrain/improved_down_estimator.h"
 #include "glog/logging.h"
 #include "gtest/gtest.h"
@@ -91,7 +92,8 @@
 }
 
 TEST(DownEstimatorTest, UkfConstantRotation) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
   EXPECT_EQ(0.0,
             (Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
@@ -112,7 +114,8 @@
 
 // Tests that the euler angles in the status message are correct.
 TEST(DownEstimatorTest, UkfEulerStatus) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
   const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
   const Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
@@ -167,7 +170,8 @@
 // that we are slightly rotated, that we eventually adjust our estimate to be
 // correct.
 TEST(DownEstimatorTest, UkfAccelCorrectsBias) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
   Eigen::Matrix<double, 3, 1> measurement;
   // Supply the accelerometer with a slightly off reading to ensure that we
@@ -192,7 +196,8 @@
 // that we are slightly rotated, that we eventually adjust our estimate to be
 // correct.
 TEST(DownEstimatorTest, UkfIgnoreBadAccel) {
-  drivetrain::DrivetrainUkf dtukf;
+  drivetrain::DrivetrainUkf dtukf(
+      drivetrain::testing::GetTestDrivetrainConfig());
   const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
   Eigen::Matrix<double, 3, 1> measurement;
   // Set up a scenario where, if we naively took the accelerometer readings, we
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 5ad175e..a9820f7 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -8,7 +8,7 @@
 #include "frc971/control_loops/drivetrain/distance_spline.h"
 #include "frc971/control_loops/drivetrain/spline.h"
 #include "frc971/control_loops/drivetrain/trajectory.h"
-#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -124,7 +124,7 @@
   Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
                             int num_distance) {
     return new Trajectory(
-        spline, ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+        spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
         num_distance);
   }
 
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index b28334c..051e71a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -138,7 +138,7 @@
       (::Eigen::DiagonalMatrix<double, 5>().diagonal()
            << 1.0 / ::std::pow(0.12, 2),
        1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
-       1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
+       1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
           .finished()
           .asDiagonal();
   const ::Eigen::DiagonalMatrix<double, 2> R =
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 43b1c30..4da3dee 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -37,7 +37,7 @@
                 ? ::std::max(100, static_cast<int>(spline_->length() / 0.0025))
                 : num_distance,
             vmax),
-      plan_segment_type_(plan_.size() - 1, SegmentType::VELOCITY_LIMITED) {}
+      plan_segment_type_(plan_.size(), SegmentType::VELOCITY_LIMITED) {}
 
 void Trajectory::LateralAccelPass() {
   for (size_t i = 0; i < plan_.size(); ++i) {
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index a6160e7..f033a89 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -75,6 +75,9 @@
     ::std::swap(coefficients_, other.coefficients_);
     X_.swap(other.X_);
     Y_.swap(other.Y_);
+    A_.swap(other.A_);
+    B_.swap(other.B_);
+    DelayedU_.swap(other.DelayedU_);
   }
 
   virtual ~StateFeedbackHybridPlant() {}
@@ -139,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.
@@ -155,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);
@@ -167,8 +172,6 @@
     DelayedU_ = U;
   }
 
-  Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
-
   Eigen::Matrix<Scalar, number_of_states, 1> Update(
       const Eigen::Matrix<Scalar, number_of_states, 1> X,
       const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
@@ -177,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:
@@ -202,6 +209,8 @@
       number_of_states, number_of_inputs, number_of_outputs>>>
       coefficients_;
 
+  Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
+
   int index_;
 
   DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
@@ -243,9 +252,13 @@
           *observers)
       : coefficients_(::std::move(*observers)) {}
 
-  HybridKalman(HybridKalman &&other)
-      : X_hat_(other.X_hat_), index_(other.index_) {
+  HybridKalman(HybridKalman &&other) : index_(other.index_) {
     ::std::swap(coefficients_, other.coefficients_);
+
+    X_hat_.swap(other.X_hat_);
+    P_.swap(other.P_);
+    Q_.swap(other.Q_);
+    R_.swap(other.R_);
   }
 
   // Getters for Q
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 20672cf..ab8afd0 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -42,6 +42,8 @@
 // position of (10, -5, 0) and a yaw of pi / 2, that suggests the robot is
 // facing straight to the left from the driver's perspective and is placed 10m
 // from the driver's station wall and 5m to the right of the center of the wall.
+// For 2020, we move the origin to be the center of the field and make positive
+// x always point towards the red alliance driver stations.
 //
 // Furthermore, Poses can be chained such that a Pose can be placed relative to
 // another Pose; the other Pose can dynamically update, thus allowing us to,
@@ -134,7 +136,7 @@
   // If new_base == nullptr, provides a Pose referenced to the global frame.
   // Note that the lifetime of new_base should be greater than the lifetime of
   // the returned object (unless new_base == nullptr).
-  TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+  [[nodiscard]] TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
 
   // Convert this pose to the heading/distance/skew numbers that we
   // traditionally use for EKF corrections.
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 1fae02b..7953474 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -167,22 +167,22 @@
     name = "spline_graph",
     srcs = [
         "color.py",
-        "spline_drawing.py",
-        "spline_writer.py",
+        "graph.py",
         "path_edit.py",
         "points.py",
-        "graph.py",
+        "spline_drawing.py",
         "spline_graph.py",
+        "spline_writer.py",
     ],
     legacy_create_init = False,
     restricted_to = ["//tools:k8"],
     visibility = ["//visibility:public"],
     deps = [
+        ":basic_window",
         ":libspline",
         ":python_init",
-        "@python_gtk",
         "@matplotlib_repo//:matplotlib2.7",
-        ":basic_window",
+        "@python_gtk",
     ],
 )
 
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 7b45215..e0b4878 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -11,18 +11,31 @@
 args = arg_parser.parse_args()
 SCREEN_SIZE = args.size
 
-WIDTH_OF_FIELD_IN_METERS = 8.258302
-
 WIDTH_OF_ROBOT = 0.65
 LENGTH_OF_ROBOT = 0.8
 
-ROBOT_SIDE_TO_BALL_CENTER = 0.15 #Placeholder value
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 # Placeholder value
 BALL_RADIUS = 0.165
-ROBOT_SIDE_TO_HATCH_PANEL = 0.1 #Placeholder value
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 # Placeholder value
 HATCH_PANEL_WIDTH = 0.4826
 
-def pxToM(p):
+FIELD = 2020
+
+if FIELD == 2019:
+    WIDTH_OF_FIELD_IN_METERS = 8.258302 # Half Field
+elif FIELD == 2020:
+    WIDTH_OF_FIELD_IN_METERS = 15.98295 # Full Field
+    LENGTH_OF_FIELD_IN_METERS = 8.21055 # Full Field
+
+def pxToM(p, length = False):
+    if(length):
+        return p * LENGTH_OF_FIELD_IN_METERS / (SCREEN_SIZE/2)
     return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
 
-def mToPx(m):
+def mToPx(m, length = False):
+    if(length):
+        return (m*(SCREEN_SIZE/2)/LENGTH_OF_FIELD_IN_METERS)
     return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
+
+def inToM(i):
+    return (i*0.0254)
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/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index 89979f0..a2ddf5d 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -3,7 +3,6 @@
 from constants import *
 import numpy as np
 
-
 def set_color(cr, color, a=1):
     if color.a == 1.0:
         cr.set_source_rgba(color.r, color.g, color.b, a)
@@ -37,6 +36,11 @@
     cr.stroke()
     set_color(cr, palette["WHITE"])
 
+def draw_circle(cr,  x, y, radius, color=palette["RED"]):
+    set_color(cr, color)
+    cr.arc(x, y, radius, 0, 2*np.pi)
+    cr.fill()
+    cr.stroke()
 
 def draw_control_points(cr, points, width=10, radius=4, color=palette["BLUE"]):
     for i in range(0, len(points)):
@@ -46,12 +50,131 @@
         cr.fill()
         set_color(cr, palette["WHITE"])
 
-
 def display_text(cr, text, widtha, heighta, widthb, heightb):
     cr.scale(widtha, -heighta)
     cr.show_text(text)
     cr.scale(widthb, -heightb)
 
+def markers(cr):
+    SHOW_MARKERS = False
+    if SHOW_MARKERS:
+        # Shield Generator Reference
+        color = palette["BLUE"]
+        yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+        # Top Left
+        draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True), 10, color)
+        # Top Right
+        draw_circle(cr, mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True), 10, color)
+        # Bottom Right
+        draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True), 10, color)
+        # Bottom Left
+        draw_circle(cr, mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True), 10, color)
+
+        # Trench Run Reference
+        color = palette["GREEN"]
+        # Bottom Trench Run
+        # Bottom Right
+        draw_circle(cr, mToPx(inToM(206.625)), yorigin, 10, color)
+        # Top Right
+        draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True), 10, color)
+        # Top Left
+        draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True), 10, color)
+        # Bottom Left
+        draw_circle(cr, mToPx(inToM(422.625)), yorigin, 10, color)
+
+        # Top Trench Run
+        # Top Right
+        draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(323.25), True), 10, color)
+        # Bottom Right
+        draw_circle(cr, mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True), 10, color)
+        # Top Left
+        draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True), 10, color)
+        # Bottom Left
+        draw_circle(cr, mToPx(inToM(422.625)), yorigin + mToPx(inToM(323.25), True), 10, color)
+        cr.stroke()
+
+def draw_init_lines(cr):
+    yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+    set_color(cr, palette["RED"])
+    cr.move_to(mToPx(inToM(120)), yorigin)
+    cr.line_to(mToPx(inToM(120)), yorigin + mToPx(8.21055, True))
+
+    cr.move_to(mToPx(inToM(505.25)), yorigin)
+    cr.line_to(mToPx(inToM(505.25)), yorigin + mToPx(8.21055, True))
+
+    cr.stroke()
+
+def draw_trench_run(cr):
+    set_color(cr, palette["GREEN"])
+    yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+    cr.move_to(mToPx(inToM(206.625)), yorigin)
+    cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True))
+
+    cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(55.5), True))
+    cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True))
+
+    cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(55.5), True))
+    cr.line_to(mToPx(inToM(422.625)), yorigin)
+
+    cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(323.25), True))
+    cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True))
+
+    cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(281.26), True))
+    cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True))
+
+    cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(281.26), True))
+    cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(323.25), True))
+
+    cr.stroke()
+
+def draw_shield_generator(cr):
+    set_color(cr, palette["BLUE"])
+    yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+
+    cr.move_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True))
+    cr.line_to(mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True))
+
+    cr.move_to(mToPx(inToM(373.616)), yorigin + mToPx(inToM(281.26), True))
+    cr.line_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True))
+
+    cr.move_to(mToPx(inToM(422.625)), yorigin + mToPx(inToM(124.67), True))
+    cr.line_to(mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True))
+
+    cr.move_to(mToPx(inToM(255.634)), yorigin + mToPx(inToM(55.5), True))
+    cr.line_to(mToPx(inToM(206.625)), yorigin + mToPx(inToM(212.097), True))
+
+    cr.stroke()
+
+def draw_control_panel(cr): # Base plates are not included
+    set_color(cr, palette["LIGHT_GREY"])
+    yorigin = 0-SCREEN_SIZE/2 # Move origin to bottom left
+    # Bottom Control Panel
+    # Top Line
+    cr.move_to(mToPx(inToM(225.624)), yorigin + mToPx(inToM(55.5), True))
+    cr.line_to(mToPx(inToM(285.624)), yorigin + mToPx(inToM(55.5), True))
+
+    # Left Line
+    cr.move_to(mToPx(inToM(225.624)),  yorigin + mToPx(inToM(55.5), True))
+    cr.line_to(mToPx(inToM(225.624)), yorigin)
+
+    # Right Line
+    cr.move_to(mToPx(inToM(285.624)), yorigin + mToPx(inToM(55.5), True))
+    cr.line_to(mToPx(inToM(285.624)), yorigin)
+
+    # Top Control Panel
+    # Bottom Line
+    cr.move_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(281.26), True))
+    cr.line_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(281.26), True))
+
+    # Right Line
+    cr.move_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(281.26), True))
+    cr.line_to(mToPx(inToM(403.616)), yorigin + mToPx(inToM(323.25), True))
+
+    # Left Line
+    cr.move_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(281.26), True))
+    cr.line_to(mToPx(inToM(343.616)), yorigin + mToPx(inToM(323.25), True))
+
+    cr.stroke()
 
 def draw_HAB(cr):
     # BASE Constants
@@ -307,7 +430,6 @@
                  mToPx(1.41605))
     cr.stroke()
 
-
 def draw_points(cr, p, size):
     for i in range(0, len(p)):
         draw_px_cross(cr, p[i][0], p[i][1], size,
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index d544cbb..b4efe72 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -32,7 +32,7 @@
         trajectory = Trajectory(dSpline)
         trajectory.Plan()
         plan = trajectory.GetPlanXVA(5.05*1e-3)
-        self.assertEqual(plan.shape, (3, 617))
+        self.assertEqual(plan.shape, (3, 624))
 
     def testLimits(self):
         """ A plan with a lower limit should take longer. """
@@ -44,7 +44,7 @@
         trajectory.LimitVelocity(0, trajectory.Length(), 3)
         trajectory.Plan()
         plan = trajectory.GetPlanXVA(5.05*1e-3)
-        self.assertEqual(plan.shape, (3, 650))
+        self.assertEqual(plan.shape, (3, 656))
 
 
 if __name__ == '__main__':
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 0d1b263..c186711 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -72,6 +72,8 @@
         self.inValue = None
         self.startSet = False
 
+        self.module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+
     """set extents on images"""
 
     def reinit_extents(self):
@@ -93,9 +95,17 @@
         return self.all_controls[self.get_index_of_nearest_point()]
 
     def draw_field_elements(self, cr):
-        draw_HAB(cr)
-        draw_rockets(cr)
-        draw_cargo_ship(cr)
+        if FIELD == 2019:
+            draw_HAB(cr)
+            draw_rockets(cr)
+            draw_cargo_ship(cr)
+        elif FIELD == 2020:
+            set_color(cr, palette["BLACK"])
+            markers(cr)
+            draw_shield_generator(cr)
+            draw_trench_run(cr)
+            draw_init_lines(cr)
+            draw_control_panel(cr)
 
     def draw_robot_at_point(self, cr, i, p, spline):
         p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
@@ -206,16 +216,16 @@
                      self.extents_y_max - self.extents_y_min)
         cr.fill()
 
-        #Drawing the switch and scale in the field
         cr.move_to(0, 50)
         cr.show_text('Press "e" to export')
         cr.show_text('Press "i" to import')
 
-        set_color(cr, palette["WHITE"])
-        cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
-        cr.fill()
         set_color(cr, palette["BLACK"])
-        cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
+        if FIELD == 2020:
+            cr.rectangle(0, mToPx(-7.991475), SCREEN_SIZE, SCREEN_SIZE/2)
+        else:
+            cr.rectangle(0, mToPx(-7.991475), SCREEN_SIZE, SCREEN_SIZE)
+            print(mToPx(-7.991475))
         cr.set_line_join(cairo.LINE_JOIN_ROUND)
         cr.stroke()
         self.draw_field_elements(cr)
@@ -312,35 +322,45 @@
             self.spline_edit = self.points.updates_for_mouse_move(
                 self.index_of_edit, self.spline_edit, self.x, self.y, difs)
 
+    def export_json(self, file_name):
+        self.path_to_export = os.path.join(self.module_path,
+                                           "spline_jsons/" + file_name)
+        if file_name[-5:] != ".json":
+            print("Error: Filename doesn't end in .json")
+        else:
+            # Will export to json file
+            self.mode = Mode.kEditing
+            exportList = [l.tolist() for l in self.points.getSplines()]
+            with open(self.path_to_export, mode='w') as points_file:
+                json.dump(exportList, points_file)
+
+    def import_json(self, file_name):
+        self.path_to_export = os.path.join(self.module_path,
+                                           "spline_jsons/" + file_name)
+        if file_name[-5:] != ".json":
+            print("Error: Filename doesn't end in .json")
+        else:
+            # import from json file
+            self.mode = Mode.kEditing
+            self.points.resetPoints()
+            self.points.resetSplines()
+            print("LOADING LOAD FROM " + file_name) # Load takes a few seconds
+            with open(self.path_to_export) as points_file:
+                self.points.setUpSplines(json.load(points_file))
+
+            self.points.update_lib_spline()
+            print("SPLINES LOADED")
+
     def do_key_press(self, event, file_name):
         keyval = Gdk.keyval_to_lower(event.keyval)
-        module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
-        self.path_to_export = os.path.join(module_path,
-                                           "spline_jsons/" + file_name)
         if keyval == Gdk.KEY_q:
             print("Found q key and exiting.")
             quit_main_loop()
-        file_name_end = file_name[-5:]
-        if file_name_end != ".json":
-            print("Error: Filename doesn't end in .json")
-        else:
-            if keyval == Gdk.KEY_e:
-                # Will export to json file
-                self.mode = Mode.kEditing
-                print('out to: ', self.path_to_export)
-                exportList = [l.tolist() for l in self.points.getSplines()]
-                with open(self.path_to_export, mode='w') as points_file:
-                    json.dump(exportList, points_file)
+        if keyval == Gdk.KEY_e:
+            export_json(file_name)
 
-            if keyval == Gdk.KEY_i:
-                # import from json file
-                self.mode = Mode.kEditing
-                self.points.resetPoints()
-                self.points.resetSplines()
-                with open(self.path_to_export) as points_file:
-                    self.points.setUpSplines(json.load(points_file))
-
-                self.points.update_lib_spline()
+        if keyval == Gdk.KEY_i:
+            import_json(file_name)
 
         if keyval == Gdk.KEY_p:
             self.mode = Mode.kPlacing
@@ -393,97 +413,7 @@
                             self.held_x = self.x
 
     def do_button_press(self, event):
-        print("button press activated")
         # Be consistent with the scaling in the drawing_area
         self.x = event.x * 2
         self.y = event.y * 2
         self.button_press_action()
-
-
-class GridWindow(Gtk.Window):
-    def method_connect(self, event, cb):
-        def handler(obj, *args):
-            cb(*args)
-
-        print("Method_connect ran")
-        self.connect(event, handler)
-
-    def mouse_move(self, event):
-        #Changes event.x and event.y to be relative to the center.
-        x = event.x - self.drawing_area.window_shape[0] / 2
-        y = self.drawing_area.window_shape[1] / 2 - event.y
-        scale = self.drawing_area.get_current_scale()
-        event.x = x / scale + self.drawing_area.center[0]
-        event.y = y / scale + self.drawing_area.center[1]
-        self.drawing_area.mouse_move(event)
-        self.queue_draw()
-
-    def button_press(self, event):
-        print("button press activated")
-        o_x = event.x
-        o_y = event.y
-        x = event.x - self.drawing_area.window_shape[0] / 2
-        y = self.drawing_area.window_shape[1] / 2 - event.y
-        scale = 2 * self.drawing_area.get_current_scale()
-        event.x = x / scale + self.drawing_area.center[0]
-        event.y = y / scale + self.drawing_area.center[1]
-        self.drawing_area.do_button_press(event)
-        event.x = o_x
-        event.y = o_y
-
-    def key_press(self, event):
-        print("key press activated")
-        self.drawing_area.do_key_press(event, self.file_name_box.get_text())
-        self.queue_draw()
-
-    def configure(self, event):
-        print("configure activated")
-        self.drawing_area.window_shape = (event.width, event.height)
-
-    def __init__(self):
-        Gtk.Window.__init__(self)
-
-        self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
-
-        flowBox = Gtk.FlowBox()
-        flowBox.set_valign(Gtk.Align.START)
-        flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
-
-        flowBox.set_valign(Gtk.Align.START)
-
-        self.add(flowBox)
-
-        container = Gtk.Fixed()
-        flowBox.add(container)
-
-        self.eventBox = Gtk.EventBox()
-        container.add(self.eventBox)
-
-        self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
-                                 | Gdk.EventMask.BUTTON_RELEASE_MASK
-                                 | Gdk.EventMask.POINTER_MOTION_MASK
-                                 | Gdk.EventMask.SCROLL_MASK
-                                 | Gdk.EventMask.KEY_PRESS_MASK)
-
-        #add the graph box
-        self.drawing_area = GTK_Widget()
-        self.eventBox.add(self.drawing_area)
-
-        self.method_connect("key-release-event", self.key_press)
-        self.method_connect("button-release-event", self.button_press)
-        self.method_connect("configure-event", self.configure)
-        self.method_connect("motion_notify_event", self.mouse_move)
-
-        self.file_name_box = Gtk.Entry()
-        self.file_name_box.set_size_request(200, 40)
-
-        self.file_name_box.set_text("File")
-        self.file_name_box.set_editable(True)
-
-        container.put(self.file_name_box, 0, 0)
-
-        self.show_all()
-
-
-window = GridWindow()
-RunApp()
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index 2fd7964..7885ec1 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -11,11 +11,10 @@
         def handler(obj, *args):
             cb(*args)
 
-        print("Method_connect ran")
         self.connect(event, handler)
 
     def mouse_move(self, event):
-        #Changes event.x and event.y to be relative to the center.
+        # Changes event.x and event.y to be relative to the center.
         x = event.x - self.drawing_area.window_shape[0] / 2
         y = self.drawing_area.window_shape[1] / 2 - event.y
         scale = self.drawing_area.get_current_scale()
@@ -25,31 +24,31 @@
         self.queue_draw()
 
     def button_press(self, event):
-        print("button press activated")
-        o_x = event.x
-        o_y = event.y
+        original_x = event.x
+        original_y = event.y
         x = event.x - self.drawing_area.window_shape[0] / 2
         y = self.drawing_area.window_shape[1] / 2 - event.y
         scale = 2 * self.drawing_area.get_current_scale()
         event.x = x / scale + self.drawing_area.center[0]
         event.y = y / scale + self.drawing_area.center[1]
         self.drawing_area.do_button_press(event)
-        event.x = o_x
-        event.y = o_y
+        event.x = original_x
+        event.y = original_y
 
     def key_press(self, event):
-        print("key press activated")
         self.drawing_area.do_key_press(event, self.file_name_box.get_text())
         self.queue_draw()
 
     def configure(self, event):
-        print("configure activated")
         self.drawing_area.window_shape = (event.width, event.height)
 
-    # handle submitting a constraint
-    def on_submit_click(self, widget):
-        self.drawing_area.inConstraint = int(self.constraint_box.get_text())
-        self.drawing_area.inValue = int(self.value_box.get_text())
+    def output_json_clicked(self, button):
+        print("OUTPUT JSON CLICKED")
+        self.drawing_area.export_json(self.file_name_box.get_text())
+
+    def input_json_clicked(self, button):
+        print("INPUT JSON CLICKED")
+        self.drawing_area.import_json(self.file_name_box.get_text())
 
     def __init__(self):
         Gtk.Window.__init__(self)
@@ -88,11 +87,22 @@
         self.file_name_box = Gtk.Entry()
         self.file_name_box.set_size_request(200, 40)
 
-        self.file_name_box.set_text("File")
+        self.file_name_box.set_text("output_file_name.json")
         self.file_name_box.set_editable(True)
 
         container.put(self.file_name_box, 0, 0)
 
+        self.output_json = Gtk.Button.new_with_label("Output")
+        self.output_json.set_size_request(100, 40)
+        self.output_json.connect("clicked", self.output_json_clicked)
+
+        self.input_json = Gtk.Button.new_with_label("Import")
+        self.input_json.set_size_request(100, 40)
+        self.input_json.connect("clicked", self.input_json_clicked)
+
+        container.put(self.output_json, 210, 0)
+        container.put(self.input_json, 320, 0)
+
         self.show_all()
 
 
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 83a54f0..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_; }
@@ -397,11 +399,11 @@
       : plant_(::std::move(other.plant_)),
         controller_(::std::move(other.controller_)),
         observer_(::std::move(other.observer_)) {
+    ff_U_.swap(other.ff_U_);
     R_.swap(other.R_);
     next_R_.swap(other.next_R_);
     U_.swap(other.U_);
     U_uncapped_.swap(other.U_uncapped_);
-    ff_U_.swap(other.ff_U_);
   }
 
   virtual ~StateFeedbackLoop() {}
@@ -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.bzl b/frc971/downloader.bzl
index aff3420..11423e0 100644
--- a/frc971/downloader.bzl
+++ b/frc971/downloader.bzl
@@ -1,7 +1,15 @@
 load("//frc971/downloader:downloader.bzl", "aos_downloader")
 load("//tools/build_rules:label.bzl", "expand_label")
 
-def robot_downloader(start_binaries, binaries = [], data = [], dirs = None, default_target = None):
+
+def robot_downloader(start_binaries,
+                     name="download",
+                     binaries=[],
+                     data=[],
+                     dirs=None,
+                     default_target=None,
+                     restricted_to=["//tools:roborio"],
+                     target_type="roborio"):
     """Sets up the standard robot download targets.
 
     Attrs:
@@ -11,27 +19,30 @@
     """
 
     aos_downloader(
-        name = "download",
-        start_srcs = [
+        name=name,
+        start_srcs=([
             "//aos:prime_start_binaries",
-        ] + start_binaries,
-        srcs = [
+        ] if target_type == "roborio" else []) + start_binaries,
+        srcs=[
             "//aos:prime_binaries",
         ] + binaries + data,
-        dirs = dirs,
-        default_target = default_target,
-        restricted_to = ["//tools:roborio"],
+        dirs=dirs,
+        target_type=target_type,
+        default_target=default_target,
+        restricted_to=restricted_to,
     )
 
     aos_downloader(
-        name = "download_stripped",
-        start_srcs = [
+        name=name + "_stripped",
+        start_srcs=([
             "//aos:prime_start_binaries_stripped",
-        ] + [expand_label(binary) + ".stripped" for binary in start_binaries],
-        srcs = [
+        ] if target_type == "roborio" else []) +
+        [expand_label(binary) + ".stripped" for binary in start_binaries],
+        srcs=[
             "//aos:prime_binaries_stripped",
         ] + [expand_label(binary) + ".stripped" for binary in binaries] + data,
-        dirs = dirs,
-        default_target = default_target,
-        restricted_to = ["//tools:roborio"],
+        dirs=dirs,
+        target_type=target_type,
+        default_target=default_target,
+        restricted_to=restricted_to,
     )
diff --git a/frc971/downloader/downloader.bzl b/frc971/downloader/downloader.bzl
index a5d1bc1..75a1a9b 100644
--- a/frc971/downloader/downloader.bzl
+++ b/frc971/downloader/downloader.bzl
@@ -7,16 +7,16 @@
             "#!/bin/bash",
             "set -e",
             'cd "${BASH_SOURCE[0]}.runfiles/%s"' % ctx.workspace_name,
-        ] + ['%s %s --dirs %s -- %s "$@"' % (
+        ] + ['%s --dir %s --target "$@" --type %s %s' % (
             ctx.executable._downloader.short_path,
-            " ".join([src.short_path for src in d.downloader_srcs]),
             d.downloader_dir,
-            ctx.attr.default_target,
+            ctx.attr.target_type,
+            " ".join([src.short_path for src in d.downloader_srcs]),
         ) for d in ctx.attr.dirs] + [
-            'exec %s %s -- %s "$@"' % (
+            'exec %s --target "$@" --type %s %s' % (
                 ctx.executable._downloader.short_path,
+                ctx.attr.target_type,
                 " ".join([src.short_path for src in all_files]),
-                ctx.attr.default_target,
             ),
         ]),
     )
@@ -57,8 +57,6 @@
   srcs: The files to download. They currently all get shoved into one folder.
   dirs: A list of aos_downloader_dirs to download too.
   start_srcs: Like srcs, except they also get put into start_list.txt.
-  default_target: The default host to download to. If not specified, defaults to
-                  roboRIO-971.local.
 """
 
 aos_downloader = rule(
@@ -76,6 +74,9 @@
             mandatory = True,
             allow_files = True,
         ),
+        "target_type": attr.string(
+            default = "roborio",
+        ),
         "dirs": attr.label_list(
             mandatory = False,
             providers = [
@@ -83,9 +84,6 @@
                 "downloader_srcs",
             ],
         ),
-        "default_target": attr.string(
-            default = "roboRIO-971-frc.local",
-        ),
     },
     executable = True,
     outputs = {
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index e4826cf..42d4a7c 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -4,6 +4,7 @@
 
 from __future__ import print_function
 
+import argparse
 import sys
 import subprocess
 import re
@@ -16,61 +17,76 @@
     PKG_URL = "http://download.ni.com/ni-linux-rt/feeds/2015/arm/ipk/cortexa9-vfpv3/" + pkg
     subprocess.check_call(["wget", PKG_URL, "-O", pkg])
     try:
-        subprocess.check_call([
-            scp_path, "-S", ssh_path, pkg,
-            ssh_target + ":/tmp/" + pkg
-        ])
-        subprocess.check_call([
-            ssh_path, ssh_target, "opkg", "install",
-            "/tmp/" + pkg
-        ])
         subprocess.check_call(
-            [ssh_path, ssh_target, "rm", "/tmp/" + pkg])
+            [scp_path, "-S", ssh_path, pkg, ssh_target + ":/tmp/" + pkg])
+        subprocess.check_call(
+            [ssh_path, ssh_target, "opkg", "install", "/tmp/" + pkg])
+        subprocess.check_call([ssh_path, ssh_target, "rm", "/tmp/" + pkg])
     finally:
         subprocess.check_call(["rm", pkg])
 
 
 def main(argv):
-    args = argv[argv.index("--") + 1:]
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--target",
+                        type=str,
+                        default="roborio-971-frc.local",
+                        help="Target to deploy code to.")
+    parser.add_argument("--type",
+                        type=str,
+                        choices=["roborio", "pi"],
+                        required=True,
+                        help="Target type for deployment")
+    parser.add_argument(
+        "--dir",
+        type=str,
+        help="Directory within robot_code to copy the files to.")
+    parser.add_argument("srcs",
+                        type=str,
+                        nargs='+',
+                        help="List of files to copy over")
+    args = parser.parse_args(argv[1:])
 
     relative_dir = ""
     recursive = False
 
-    if "--dirs" in argv:
-        dirs_index = argv.index("--dirs")
-        srcs = argv[1:dirs_index]
-        relative_dir = argv[dirs_index + 1]
+    srcs = args.srcs
+    if args.dir is not None:
+        relative_dir = args.dir
         recursive = True
-    else:
-        srcs = argv[1:argv.index("--")]
 
-    ROBORIO_TARGET_DIR = "/home/admin/robot_code"
-    ROBORIO_USER = "admin"
-
-    target_dir = ROBORIO_TARGET_DIR
-    user = ROBORIO_USER
-    destination = args[-1]
+    destination = args.target
 
     result = re.match("(?:([^:@]+)@)?([^:@]+)(?::([^:@]+))?", destination)
     if not result:
-        print(
-            "Not sure how to parse destination \"%s\"!" % destination,
-            file=sys.stderr)
+        print("Not sure how to parse destination \"%s\"!" % destination,
+              file=sys.stderr)
         return 1
+    user = None
     if result.group(1):
         user = result.group(1)
     hostname = result.group(2)
+
     if result.group(3):
         target_dir = result.group(3)
 
+    if user is None:
+        if args.type == "pi":
+          user = "pi"
+        elif args.type == "roborio":
+          user = "admin"
+    target_dir = "/home/" + user + "/robot_code"
+
     ssh_target = "%s@%s" % (user, hostname)
 
     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"
+        "external/rsync/usr/bin/rsync", "-e", ssh_path, "-c", "-v", "-z",
+        "--copy-links"
     ] + srcs + ["%s:%s/%s" % (ssh_target, target_dir, relative_dir)])
     try:
         subprocess.check_call(rsync_cmd)
@@ -88,12 +104,11 @@
             raise e
 
     if not recursive:
-        subprocess.check_call(
-            (ssh_path, ssh_target, "&&".join([
-                "chmod u+s %s/starter_exe" % target_dir,
-                "echo \'Done moving new executables into place\'",
-                "bash -c \'sync && sync && sync\'",
-            ])))
+        subprocess.check_call((ssh_path, ssh_target, "&&".join([
+            "chmod u+s %s/starter_exe" % target_dir,
+            "echo \'Done moving new executables into place\'",
+            "bash -c \'sync && sync && sync\'",
+        ])))
 
 
 if __name__ == "__main__":
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index b13b43d..9e82d13 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -35,15 +35,23 @@
          accel_averager_.GetRange() < kAccelMaxVariation;
 }
 
-void ImuZeroer::ProcessMeasurement(const IMUValues &values) {
+void ImuZeroer::InsertAndProcessMeasurement(const IMUValues &values) {
+  InsertMeasurement(values);
+  ProcessMeasurements();
+}
+
+void ImuZeroer::InsertMeasurement(const IMUValues &values) {
   last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
   gyro_averager_.AddData(last_gyro_sample_);
   last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
                            values.accelerometer_z();
   accel_averager_.AddData(last_accel_sample_);
+}
+
+void ImuZeroer::ProcessMeasurements() {
   if (GyroZeroReady() && AccelZeroReady()) {
     ++good_iters_;
-    if (good_iters_ > kSamplesToAverage / 4) {
+    if (good_iters_ > kSamplesToAverage / 40) {
       const Eigen::Vector3d current_gyro_average = gyro_averager_.GetAverage();
       constexpr double kAverageUpdateWeight = 0.05;
       if (num_zeroes_ > 0) {
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index b55c6dd..fd82571 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -22,7 +22,10 @@
   ImuZeroer();
   bool Zeroed() const;
   bool Faulted() const;
-  void ProcessMeasurement(const IMUValues &values);
+  void InsertMeasurement(const IMUValues &values);
+  // PErforms the heavier-duty processing for managing zeroing.
+  void ProcessMeasurements();
+  void InsertAndProcessMeasurement(const IMUValues &values);
   Eigen::Vector3d GyroOffset() const;
   Eigen::Vector3d ZeroedGyro() const;
   Eigen::Vector3d ZeroedAccel() const;
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index eee3f57..e97fbe2 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -32,7 +32,7 @@
   ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
   // A measurement before we are zeroed should just result in the measurement
   // being passed through without modification.
-  zeroer.ProcessMeasurement(
+  zeroer.InsertAndProcessMeasurement(
       MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
   ASSERT_FALSE(zeroer.Zeroed());
   ASSERT_FALSE(zeroer.Faulted());
@@ -50,7 +50,7 @@
   ImuZeroer zeroer;
   ASSERT_FALSE(zeroer.Zeroed());
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
   }
   ASSERT_TRUE(zeroer.Zeroed());
@@ -68,7 +68,7 @@
   ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
   // If we get another measurement offset by {1, 1, 1} we should read the result
   // as {1, 1, 1}.
-  zeroer.ProcessMeasurement(
+  zeroer.InsertAndProcessMeasurement(
       MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
   ASSERT_FALSE(zeroer.Faulted());
   ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
@@ -82,7 +82,7 @@
   ImuZeroer zeroer;
   ASSERT_FALSE(zeroer.Zeroed());
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.1, 0.2, 0.3}, {4, 5, 6}).message());
     ASSERT_FALSE(zeroer.Zeroed());
   }
@@ -97,10 +97,9 @@
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
     const double offset =
         (static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 0.001;
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
-                        {4 + offset, 5 + offset, 6 + offset})
-            .message());
+                        {4 + offset, 5 + offset, 6 + offset}).message());
   }
   ASSERT_TRUE(zeroer.Zeroed());
   ASSERT_FALSE(zeroer.Faulted());
@@ -109,7 +108,7 @@
   ASSERT_NEAR(0.03, zeroer.GyroOffset().z(), 1e-3);
   // If we get another measurement offset by {0.01, 0.01, 0.01} we should read
   // the result as {0.01, 0.01, 0.01}.
-  zeroer.ProcessMeasurement(
+  zeroer.InsertAndProcessMeasurement(
       MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
   ASSERT_FALSE(zeroer.Faulted());
   ASSERT_NEAR(0.01, zeroer.ZeroedGyro().x(), 1e-3);
@@ -128,10 +127,9 @@
     ASSERT_FALSE(zeroer.Zeroed());
     const double offset =
         (static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 1.0;
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
-                        {4 + offset, 5 + offset, 6 + offset})
-            .message());
+                        {4 + offset, 5 + offset, 6 + offset}).message());
   }
   ASSERT_FALSE(zeroer.Zeroed());
   ASSERT_FALSE(zeroer.Faulted());
@@ -143,14 +141,14 @@
   ImuZeroer zeroer;
   ASSERT_FALSE(zeroer.Zeroed());
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
   }
   ASSERT_TRUE(zeroer.Zeroed());
   ASSERT_FALSE(zeroer.Faulted())
       << "We should not fault until we complete a second cycle of zeroing.";
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01, 0.05, 0.03}, {4, 5, 6}).message());
   }
   ASSERT_TRUE(zeroer.Faulted());
@@ -161,12 +159,12 @@
   ImuZeroer zeroer;
   ASSERT_FALSE(zeroer.Zeroed());
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01, 0.02, 0.03}, {4, 5, 6}).message());
   }
   ASSERT_TRUE(zeroer.Zeroed());
   for (size_t ii = 0; ii < kMinSamplesToZero; ++ii) {
-    zeroer.ProcessMeasurement(
+    zeroer.InsertAndProcessMeasurement(
         MakeMeasurement({0.01, 0.020001, 0.03}, {4, 5, 6}).message());
   }
   ASSERT_FALSE(zeroer.Faulted());
diff --git a/third_party/BUILD b/third_party/BUILD
index 5b2ca31..bcba978 100644
--- a/third_party/BUILD
+++ b/third_party/BUILD
@@ -113,4 +113,3 @@
         "roborio": ["@webrtc_rio//:webrtc"],
     }),
 )
-
diff --git a/third_party/flatbuffers/build_defs.bzl b/third_party/flatbuffers/build_defs.bzl
index 6ac1d87..f4c1dc7 100644
--- a/third_party/flatbuffers/build_defs.bzl
+++ b/third_party/flatbuffers/build_defs.bzl
@@ -282,7 +282,7 @@
         been parsed. As such, we just force the user to manually specify
         things.
     """
-    python_files = ["%s/%s.py" % (namespace.replace('.', '/'), table) for table in tables]
+    python_files = ["%s/%s.py" % (namespace.replace(".", "/"), table) for table in tables]
 
     srcs_lib = "%s_srcs" % (name)
     flatbuffer_library_public(
diff --git a/third_party/gperftools/BUILD b/third_party/gperftools/BUILD
index 25a8740..7f5e9d6 100644
--- a/third_party/gperftools/BUILD
+++ b/third_party/gperftools/BUILD
@@ -86,7 +86,7 @@
     "-DPERFTOOLS_DLL_DECL=",
     "-DSTDC_HEADERS=1",
     "-DSTL_NAMESPACE=std",
-    "-DPACKAGE_STRING=\\\"gperftools\ 2.4\\\"",
+    "-DPACKAGE_STRING=\\\"gperftools\\ 2.4\\\"",
     "-DPACKAGE_BUGREPORT=\\\"http://www.frc971.org/contact\\\"",
     "-DPACKAGE_VERSION=\\\"2.4\\\"",
 ] + cpu_select({
@@ -141,7 +141,7 @@
         "-lrt",
         "-lpthread",
     ],
-    nocopts = "-std=gnu\+\+1y",
+    nocopts = "-std=gnu\\+\\+1y",
     visibility = ["//visibility:public"],
     deps = [
         "//third_party/empty_config_h",
diff --git a/third_party/libevent/BUILD b/third_party/libevent/BUILD
index da05f3c..347ec0b 100644
--- a/third_party/libevent/BUILD
+++ b/third_party/libevent/BUILD
@@ -148,7 +148,7 @@
         "_EVENT_STDC_HEADERS=1",
         "_EVENT_TIME_WITH_SYS_TIME=1",
         "_EVENT_NUMERIC_VERSION=0x02001600",
-        '_EVENT_VERSION=\\"2.0.22-stable\\"',
+        "_EVENT_VERSION=\\\"2.0.22-stable\\\"",
     ] + address_size_select({
         "32": [
             "_EVENT_SIZEOF_LONG_LONG=4",
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 1459248..c5a2d18 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -84,7 +84,7 @@
         });
 
     // Run for enough time to allow the gyro/imu zeroing code to run.
-    RunFor(std::chrono::seconds(10));
+    RunFor(std::chrono::seconds(15));
   }
 
   virtual ~LocalizedDrivetrainTest() {}
diff --git a/y2020/BUILD b/y2020/BUILD
index aa5f16e..320a0cf 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -4,10 +4,14 @@
 load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
 
 robot_downloader(
+    binaries = [
+        ":setpoint_setter",
+    ],
     data = [
         ":config.json",
     ],
     start_binaries = [
+        "//aos/events/logging:logger_main",
         ":joystick_reader",
         ":wpilib_interface",
         "//aos/network:message_bridge_client",
@@ -18,6 +22,24 @@
     ],
 )
 
+robot_downloader(
+    name = "pi_download",
+    data = [
+        ":config.json",
+    ],
+    dirs = [
+        "//y2020/www:www_files",
+    ],
+    restricted_to = ["//tools:armhf-debian"],
+    start_binaries = [
+        "//aos/network:message_bridge_client",
+        "//aos/network:message_bridge_server",
+        "//y2020/vision:camera_reader",
+        "//aos/network:web_proxy_main",
+    ],
+    target_type = "pi",
+)
+
 cc_library(
     name = "constants",
     srcs = [
@@ -92,6 +114,7 @@
         ":joystick_reader.cc",
     ],
     deps = [
+        ":setpoint_fbs",
         "//aos:init",
         "//aos/actions:action_lib",
         "//aos/input:action_joystick_input",
@@ -112,6 +135,7 @@
     name = "config",
     src = "y2020.json",
     flatbuffers = [
+        ":setpoint_fbs",
         "//aos/network:message_bridge_client_fbs",
         "//aos/network:message_bridge_server_fbs",
         "//aos/network:timestamp_fbs",
@@ -121,6 +145,7 @@
         "//y2020/control_loops/superstructure:superstructure_position_fbs",
         "//y2020/control_loops/superstructure:superstructure_status_fbs",
         "//y2020/vision/sift:sift_fbs",
+        "//y2020/vision/sift:sift_training_fbs",
         "//y2020/vision:vision_fbs",
     ],
     visibility = ["//visibility:public"],
@@ -143,9 +168,30 @@
     srcs = ["web_proxy.sh"],
     data = [
         ":config.json",
+        "//y2020/www:field_main_bundle",
         "//aos/network:web_proxy_main",
         "//y2020/www:files",
         "//y2020/www:flatbuffers",
-        "//y2020/www:main_bundle",
+        "//y2020/www:camera_main_bundle",
+    ],
+)
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+flatbuffer_cc_library(
+    name = "setpoint_fbs",
+    srcs = [
+        "setpoint.fbs",
+    ],
+    gen_reflections = 1,
+)
+
+cc_binary(
+    name = "setpoint_setter",
+    srcs = ["setpoint_setter.cc"],
+    deps = [
+        ":setpoint_fbs",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
     ],
 )
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 86eed14..3498a3e 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -59,16 +59,16 @@
               {longitudinal_constraint_offset, lateral_constraint_offset,
                voltage_constraint_offset});
 
-  const float startx = 0.4;
-  const float starty = 3.4;
+  const float startx = 0.0;
+  const float starty = 0.05;
   flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
-      builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
-                                           0.6f + startx, 0.4f + startx,
-                                           0.4f + startx, 1.0f + startx});
+      builder->fbb()->CreateVector<float>({0.0f + startx, 0.8f + startx,
+                                           0.8f + startx, 1.2f + startx,
+                                           1.2f + startx, 2.0f + startx});
   flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
       builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
-                                           starty - 0.3f, starty - 0.7f,
-                                           starty - 1.0f, starty - 1.0f});
+                                           starty - 0.1f, starty - 0.2f,
+                                           starty - 0.3f, starty - 0.3f});
 
   frc971::MultiSpline::Builder multispline_builder =
       builder->MakeBuilder<frc971::MultiSpline>();
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index d015c65..e87e064 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -8,6 +8,7 @@
 #include "aos/logging/logging.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
 #include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/actors/auto_splines.h"
 
 namespace y2020 {
 namespace actors {
@@ -19,17 +20,43 @@
 
 AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
     : frc971::autonomous::BaseAutonomousActor(
-          event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+          event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+      localizer_control_sender_(event_loop->MakeSender<
+          ::frc971::control_loops::drivetrain::LocalizerControl>(
+          "/drivetrain")) {}
 
 void AutonomousActor::Reset() {
   InitializeEncoders();
   ResetDrivetrain();
+
+  {
+    auto builder = localizer_control_sender_.MakeBuilder();
+
+    LocalizerControl::Builder localizer_control_builder =
+        builder.MakeBuilder<LocalizerControl>();
+    // TODO(james): Set starting position based on the alliance.
+    localizer_control_builder.add_x(0.0);
+    localizer_control_builder.add_y(0.0);
+    localizer_control_builder.add_theta(0.0);
+    localizer_control_builder.add_theta_uncertainty(0.00001);
+    if (!builder.Send(localizer_control_builder.Finish())) {
+      AOS_LOG(ERROR, "Failed to reset localizer.\n");
+    }
+  }
 }
 
 bool AutonomousActor::RunAction(
     const ::frc971::autonomous::AutonomousActionParams *params) {
   Reset();
 
+  SplineHandle spline1 =
+      PlanSpline(AutonomousSplines::BasicSSpline, SplineDirection::kForward);
+
+  if (!spline1.WaitForPlan()) return true;
+  spline1.Start();
+
+  if (!spline1.WaitForSplineDistanceRemaining(0.02)) return true;
+
   AOS_LOG(INFO, "Params are %d\n", params->mode());
   return true;
 }
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 3bfa610..d86feab 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -6,6 +6,7 @@
 #include "frc971/autonomous/base_autonomous_actor.h"
 #include "frc971/control_loops/control_loops_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
 
 namespace y2020 {
 namespace actors {
@@ -19,6 +20,9 @@
 
  private:
   void Reset();
+
+  ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+      localizer_control_sender_;
 };
 
 }  // namespace actors
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/BUILD b/y2020/control_loops/drivetrain/BUILD
index 03d2dc2..1bd760f 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -59,6 +59,7 @@
     hdrs = ["localizer.h"],
     deps = [
         "//aos/containers:ring_buffer",
+        "//aos/network:message_bridge_server_fbs",
         "//frc971/control_loops:profiled_subsystem_fbs",
         "//frc971/control_loops/drivetrain:hybrid_ekf",
         "//frc971/control_loops/drivetrain:localizer",
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
index 62679bc..b919902 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -25,8 +25,8 @@
   static DrivetrainConfig<double> kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
-      ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
-      ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+      ::frc971::control_loops::drivetrain::GyroType::IMU_X_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_Z,
 
       drivetrain::MakeDrivetrainLoop,
       drivetrain::MakeVelocityDrivetrainLoop,
@@ -53,8 +53,8 @@
       1.2 /* quickturn_wheel_multiplier */,
       1.2 /* wheel_multiplier */,
       true /*pistol_grip_shift_enables_line_follow*/,
-      (Eigen::Matrix<double, 3, 3>() << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
-       1.0)
+      (Eigen::Matrix<double, 3, 3>() << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
+       0.0)
           .finished() /*imu_transform*/,
   };
 
diff --git a/y2020/control_loops/drivetrain/drivetrain_main.cc b/y2020/control_loops/drivetrain/drivetrain_main.cc
index 7159118..7804814 100644
--- a/y2020/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_main.cc
@@ -6,7 +6,8 @@
 
 using ::frc971::control_loops::drivetrain::DrivetrainLoop;
 
-int main() {
+int main(int argc, char *argv[]) {
+  ::aos::InitGoogle(&argc, &argv);
   ::aos::InitNRT();
 
   aos::FlatbufferDetachedBuffer<aos::Configuration> config =
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index af6a841..faf2cef 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -41,15 +41,19 @@
                             "frc971.control_loops.drivetrain.Output");
   reader.Register();
 
+  const aos::Node *node = nullptr;
+  if (aos::configuration::MultiNode(reader.configuration())) {
+    node = aos::configuration::GetNode(reader.configuration(), "roborio");
+  }
+
   aos::logger::DetachedBufferWriter file_writer(FLAGS_output_file);
   std::unique_ptr<aos::EventLoop> log_writer_event_loop =
-      reader.event_loop_factory()->MakeEventLoop("log_writer");
+      reader.event_loop_factory()->MakeEventLoop("log_writer", node);
   log_writer_event_loop->SkipTimingReport();
-  CHECK(nullptr == log_writer_event_loop->node());
   aos::logger::Logger writer(&file_writer, log_writer_event_loop.get());
 
   std::unique_ptr<aos::EventLoop> drivetrain_event_loop =
-      reader.event_loop_factory()->MakeEventLoop("drivetrain");
+      reader.event_loop_factory()->MakeEventLoop("drivetrain", node);
   drivetrain_event_loop->SkipTimingReport();
 
   y2020::control_loops::drivetrain::Localizer localizer(
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 3e391c8..6033184 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -52,13 +52,19 @@
         reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
     drivetrain_event_loop_->SkipTimingReport();
 
+    frc971::control_loops::drivetrain::DrivetrainConfig<double> config =
+        GetDrivetrainConfig();
+    // Make the modification required to the imu transform to work with the 2016
+    // logs...
+    config.imu_transform << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
+    config.gyro_type = frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
+
     localizer_ =
         std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
-            drivetrain_event_loop_.get(), GetDrivetrainConfig());
+            drivetrain_event_loop_.get(), config);
     drivetrain_ =
         std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
-            GetDrivetrainConfig(), drivetrain_event_loop_.get(),
-            localizer_.get());
+            config, drivetrain_event_loop_.get(), localizer_.get());
 
     test_event_loop_ =
         reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
@@ -88,7 +94,7 @@
   ASSERT_TRUE(status_fetcher_->has_x());
   ASSERT_TRUE(status_fetcher_->has_y());
   ASSERT_TRUE(status_fetcher_->has_theta());
-  EXPECT_LT(std::abs(status_fetcher_->x()), 0.1);
+  EXPECT_LT(std::abs(status_fetcher_->x()), 0.25);
   // Because the encoders should not be affecting the y or yaw axes, expect a
   // reasonably precise result (although, since this is a real worl dtest, the
   // robot probably did actually move be some non-zero amount).
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b11f6f..d26552a 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -22,13 +22,22 @@
   }
   return result;
 }
+
+// Indices of the pis to use.
+const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+
 }  // namespace
 
 Localizer::Localizer(
     aos::EventLoop *event_loop,
     const frc971::control_loops::drivetrain::DrivetrainConfig<double>
         &dt_config)
-    : event_loop_(event_loop), dt_config_(dt_config), ekf_(dt_config) {
+    : event_loop_(event_loop),
+      dt_config_(dt_config),
+      ekf_(dt_config),
+      clock_offset_fetcher_(
+          event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+              "/aos")) {
   // TODO(james): This doesn't really need to be a watcher; we could just use a
   // fetcher for the superstructure status.
   // This probably should be a Fetcher instead of a Watcher, but this
@@ -40,9 +49,16 @@
                              HandleSuperstructureStatus(status);
                            });
 
-  image_fetchers_.emplace_back(
-      event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
-          "/pi1/camera"));
+  event_loop->OnRun([this, event_loop]() {
+    ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+                           ekf_.P());
+  });
+
+  for (const auto &pi : kPisToUse) {
+    image_fetchers_.emplace_back(
+        event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
+            "/" + pi + "/camera"));
+  }
 
   target_selector_.set_has_target(false);
 }
@@ -78,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,
@@ -88,10 +105,30 @@
 }
 
 void Localizer::HandleImageMatch(
-    const frc971::vision::sift::ImageMatchResult &result) {
-  // TODO(james): compensate for capture time across multiple nodes correctly.
+    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() == pi) {
+        monotonic_offset =
+            std::chrono::nanoseconds(connection->monotonic_offset());
+        break;
+      }
+    }
+  }
   aos::monotonic_clock::time_point capture_time(
-      std::chrono::nanoseconds(result.image_monotonic_timestamp_ns()));
+      std::chrono::nanoseconds(result.image_monotonic_timestamp_ns()) -
+      monotonic_offset);
+  VLOG(1) << "Got monotonic offset of "
+          << aos::time::DurationInSeconds(monotonic_offset)
+          << " when at time of " << event_loop_->monotonic_now()
+          << " and capture time estimate of " << capture_time;
+  if (capture_time > event_loop_->monotonic_now()) {
+    LOG(WARNING) << "Got camera frame from the future.";
+    return;
+  }
   CHECK(result.has_camera_calibration());
   // Per the ImageMatchResult specification, we can actually determine whether
   // the camera is the turret camera just from the presence of the
@@ -162,6 +199,12 @@
     H(0, StateIdx::kX) = 1;
     H(1, StateIdx::kY) = 1;
     H(2, StateIdx::kTheta) = 1;
+    // If the heading is off by too much, assume that we got a false-positive
+    // and don't use the correction.
+    if (std::abs(aos::math::DiffAngle(theta(), Z(2))) > M_PI_2) {
+      AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
+      return;
+    }
     ekf_.Correct(Z, nullptr, {}, [H, Z](const State &X, const Input &) {
                                    Eigen::Vector3d Zhat = H * X;
                                    // In order to deal with wrapping of the
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 0b79361..1d6f816 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -1,8 +1,11 @@
 #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"
 #include "frc971/control_loops/drivetrain/hybrid_ekf.h"
 #include "frc971/control_loops/drivetrain/localizer.h"
 #include "y2020/control_loops/superstructure/superstructure_status_generated.h"
@@ -68,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.
@@ -86,6 +90,8 @@
   std::vector<aos::Fetcher<frc971::vision::sift::ImageMatchResult>>
       image_fetchers_;
 
+  aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+
   // Buffer of recent turret data--this is used so that when we receive a camera
   // frame from the turret, we can back out what the turret angle was at that
   // time.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 1e72c39..c47faa7 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -4,6 +4,7 @@
 
 #include "aos/controls/control_loop_test.h"
 #include "aos/events/logging/logger.h"
+#include "aos/network/message_bridge_server_generated.h"
 #include "aos/network/team_number.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
@@ -84,12 +85,13 @@
   locations.push_back(H);
   return locations;
 }
+
+constexpr std::chrono::seconds kPiTimeOffset(10);
 }  // namespace
 
 namespace chrono = std::chrono;
 using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
 using frc971::control_loops::drivetrain::DrivetrainLoop;
-using frc971::control_loops::drivetrain::testing::GetTestDrivetrainConfig;
 using aos::monotonic_clock;
 
 class LocalizedDrivetrainTest : public aos::testing::ControlLoopTest {
@@ -113,6 +115,9 @@
         superstructure_status_sender_(
             test_event_loop_->MakeSender<superstructure::Status>(
                 "/superstructure")),
+        server_statistics_sender_(
+            test_event_loop_->MakeSender<aos::message_bridge::ServerStatistics>(
+                "/aos")),
         drivetrain_event_loop_(MakeEventLoop("drivetrain", roborio_)),
         dt_config_(GetTest2020DrivetrainConfig()),
         pi1_event_loop_(MakeEventLoop("test", pi1_)),
@@ -123,8 +128,10 @@
         drivetrain_plant_event_loop_(MakeEventLoop("plant", roborio_)),
         drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
         last_frame_(monotonic_now()) {
+    event_loop_factory()->GetNodeEventLoopFactory(pi1_)->SetDistributedOffset(
+        kPiTimeOffset);
+
     set_team_id(frc971::control_loops::testing::kTeamNumber);
-    SetStartingPosition({3.0, 2.0, 0.0});
     set_battery_voltage(12.0);
 
     if (!FLAGS_output_file.empty()) {
@@ -151,6 +158,29 @@
 
     test_event_loop_->AddPhasedLoop(
         [this](int) {
+          auto builder = server_statistics_sender_.MakeBuilder();
+          auto name_offset = builder.fbb()->CreateString("pi1");
+          auto node_builder = builder.MakeBuilder<aos::Node>();
+          node_builder.add_name(name_offset);
+          auto node_offset = node_builder.Finish();
+          auto connection_builder =
+              builder.MakeBuilder<aos::message_bridge::ServerConnection>();
+          connection_builder.add_node(node_offset);
+          connection_builder.add_monotonic_offset(
+              chrono::duration_cast<chrono::nanoseconds>(-kPiTimeOffset)
+                  .count());
+          auto connection_offset = connection_builder.Finish();
+          auto connections_offset =
+              builder.fbb()->CreateVector(&connection_offset, 1);
+          auto statistics_builder =
+              builder.MakeBuilder<aos::message_bridge::ServerStatistics>();
+          statistics_builder.add_connections(connections_offset);
+          builder.Send(statistics_builder.Finish());
+        },
+        chrono::milliseconds(500));
+
+    test_event_loop_->AddPhasedLoop(
+        [this](int) {
           // Also use the opportunity to send out turret messages.
           UpdateTurretPosition();
           auto builder = superstructure_status_sender_.MakeBuilder();
@@ -167,6 +197,8 @@
         },
         chrono::milliseconds(5));
 
+    test_event_loop_->OnRun([this]() { SetStartingPosition({3.0, 2.0, 0.0}); });
+
     // Run for enough time to allow the gyro/imu zeroing code to run.
     RunFor(std::chrono::seconds(10));
   }
@@ -258,7 +290,10 @@
 
     frame->image_monotonic_timestamp_ns =
         chrono::duration_cast<chrono::nanoseconds>(
-            monotonic_now().time_since_epoch())
+            event_loop_factory()
+                ->GetNodeEventLoopFactory(pi1_)
+                ->monotonic_now()
+                .time_since_epoch())
             .count();
     frame->camera_calibration.reset(new CameraCalibrationT());
     {
@@ -301,6 +336,7 @@
   aos::Fetcher<Goal> drivetrain_goal_fetcher_;
   aos::Sender<LocalizerControl> localizer_control_sender_;
   aos::Sender<superstructure::Status> superstructure_status_sender_;
+  aos::Sender<aos::message_bridge::ServerStatistics> server_statistics_sender_;
 
   std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
   const frc971::control_loops::drivetrain::DrivetrainConfig<double>
@@ -381,6 +417,22 @@
   EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
 }
 
+// Tests that we can drive in a straight line and have things estimate
+// correctly.
+TEST_F(LocalizedDrivetrainTest, NoCameraUpdateStraightLine) {
+  SetEnabled(true);
+  set_enable_cameras(false);
+  EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
+
+  SendGoal(1.0, 1.0);
+
+  RunFor(chrono::seconds(1));
+  VerifyNearGoal();
+  // Due to accelerometer drift, the straight-line driving tends to be less
+  // accurate...
+  EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
+}
+
 // Tests that camera updates with a perfect models results in no errors.
 TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
   SetEnabled(true);
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index 923d239..e218571 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -133,8 +133,8 @@
     legacy_create_init = False,
     restricted_to = ["//tools:k8"],
     deps = [
-        ":python_init",
         ":flywheel",
+        ":python_init",
         "//external:python-gflags",
         "//external:python-glog",
     ],
@@ -148,8 +148,8 @@
     legacy_create_init = False,
     restricted_to = ["//tools:k8"],
     deps = [
-        ":python_init",
         ":flywheel",
+        ":python_init",
         "//external:python-gflags",
         "//external:python-glog",
     ],
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/BUILD b/y2020/control_loops/superstructure/BUILD
index d81b6b5..4534404 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -62,8 +62,11 @@
         ":superstructure_status_fbs",
         "//aos/controls:control_loop",
         "//aos/events:event_loop",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
         "//y2020:constants",
         "//y2020/control_loops/superstructure/shooter",
+        "//y2020/control_loops/superstructure/turret:aiming",
     ],
 )
 
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 39c913e..8947f85 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,11 @@
       hood_(constants::GetValues().hood),
       intake_joint_(constants::GetValues().intake),
       turret_(constants::GetValues().turret.subsystem_params),
-      shooter_() {
+      drivetrain_status_fetcher_(
+          event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+              "/drivetrain")),
+      joystick_state_fetcher_(
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -34,6 +38,22 @@
   const aos::monotonic_clock::time_point position_timestamp =
       event_loop()->context().monotonic_event_time;
 
+  if (drivetrain_status_fetcher_.Fetch()) {
+    aos::Alliance alliance = aos::Alliance::kInvalid;
+    if (joystick_state_fetcher_.Fetch()) {
+      alliance = joystick_state_fetcher_->alliance();
+    }
+    const turret::Aimer::WrapMode mode =
+        (unsafe_goal != nullptr && unsafe_goal->shooting())
+            ? turret::Aimer::WrapMode::kAvoidWrapping
+            : turret::Aimer::WrapMode::kAvoidEdges;
+    aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode,
+                  turret::Aimer::ShotMode::kShootOnTheFly);
+  }
+
+  const flatbuffers::Offset<AimerStatus> aimer_status_offset =
+      aimer_.PopulateStatus(status->fbb());
+
   OutputT output_struct;
 
   flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -42,6 +62,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,
@@ -49,10 +93,14 @@
           output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
           status->fbb());
 
+  const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+      *turret_goal = unsafe_goal != nullptr ? (unsafe_goal->turret_tracking()
+                                                   ? aimer_.TurretGoal()
+                                                   : unsafe_goal->turret())
+                                            : nullptr;
   flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
       turret_status_offset = turret_.Iterate(
-          unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
-          position->turret(),
+          turret_goal, position->turret(),
           output != nullptr ? &(output_struct.turret_voltage) : nullptr,
           status->fbb());
 
@@ -64,19 +112,40 @@
 
   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);
+    output_struct.turret_voltage =
+        std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
+                   turret_.operating_voltage());
+
+    // 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() ||
@@ -92,12 +161,27 @@
   status_builder.add_intake(intake_status_offset);
   status_builder.add_turret(turret_status_offset);
   status_builder.add_shooter(shooter_status_offset);
+  status_builder.add_aimer(aimer_status_offset);
 
   status->Send(status_builder.Finish());
 
   if (output != nullptr) {
     if (unsafe_goal) {
-      output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
+      output_struct.washing_machine_spinner_voltage = 0.0;
+      if (unsafe_goal->shooting()) {
+        if (shooter_.ready() &&
+            unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
+            unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+          output_struct.feeder_voltage = 12.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();
+      }
     } 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..1a9d401 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -3,6 +3,8 @@
 
 #include "aos/controls/control_loop.h"
 #include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "y2020/constants.h"
 #include "y2020/control_loops/superstructure/shooter/shooter.h"
 #include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
@@ -10,6 +12,7 @@
 #include "y2020/control_loops/superstructure/superstructure_position_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_status_generated.h"
 #include "y2020/control_loops/superstructure/climber.h"
+#include "y2020/control_loops/superstructure/turret/aiming.h"
 
 namespace y2020 {
 namespace control_loops {
@@ -21,6 +24,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,
@@ -45,8 +56,19 @@
   AbsoluteEncoderSubsystem intake_joint_;
   PotAndAbsoluteEncoderSubsystem turret_;
   shooter::Shooter shooter_;
+  turret::Aimer aimer_;
+
+  aos::Fetcher<frc971::control_loops::drivetrain::Status>
+      drivetrain_status_fetcher_;
+  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
 
   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..e8f1f8d 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -36,6 +36,7 @@
     CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
 using ::frc971::control_loops::PositionSensorSimulator;
 using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef ::frc971::control_loops::drivetrain::Status DrivetrainStatus;
 typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
 typedef Superstructure::PotAndAbsoluteEncoderSubsystem
     PotAndAbsoluteEncoderSubsystem;
@@ -72,7 +73,6 @@
             event_loop_->MakeFetcher<Status>("/superstructure")),
         superstructure_output_fetcher_(
             event_loop_->MakeFetcher<Output>("/superstructure")),
-
         hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
         hood_encoder_(constants::GetValues()
                           .hood.zeroing_constants.one_revolution_distance),
@@ -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
@@ -386,6 +398,10 @@
             test_event_loop_->MakeFetcher<Output>("/superstructure")),
         superstructure_position_fetcher_(
             test_event_loop_->MakeFetcher<Position>("/superstructure")),
+        drivetrain_status_sender_(
+            test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+        joystick_state_sender_(
+            test_event_loop_->MakeSender<aos::JoystickState>("/aos")),
         superstructure_event_loop_(MakeEventLoop("superstructure", roborio_)),
         superstructure_(superstructure_event_loop_.get()),
         superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
@@ -494,6 +510,8 @@
   ::aos::Fetcher<Status> superstructure_status_fetcher_;
   ::aos::Fetcher<Output> superstructure_output_fetcher_;
   ::aos::Fetcher<Position> superstructure_position_fetcher_;
+  ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+  ::aos::Sender<aos::JoystickState> joystick_state_sender_;
 
   // Create a control loop and simulation.
   ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -667,7 +685,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 +769,7 @@
   }
 
   // Give it a lot of time to get there.
-  RunFor(chrono::seconds(9));
+  RunFor(chrono::seconds(15));
 
   VerifyNearGoal();
 }
@@ -814,6 +833,79 @@
   VerifyNearGoal();
 }
 
+class SuperstructureAllianceTest
+    : public SuperstructureTest,
+      public ::testing::WithParamInterface<aos::Alliance> {};
+
+// Tests that the turret switches to auto-aiming when we set turret_tracking to
+// true.
+TEST_P(SuperstructureAllianceTest, TurretAutoAim) {
+  SetEnabled(true);
+  // Set a reasonable goal.
+  const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+
+  WaitUntilZeroed();
+
+  constexpr double kShotAngle = 1.0;
+  {
+    auto builder = joystick_state_sender_.MakeBuilder();
+
+    aos::JoystickState::Builder joystick_builder =
+        builder.MakeBuilder<aos::JoystickState>();
+
+    joystick_builder.add_alliance(GetParam());
+
+    ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
+  }
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_turret_tracking(true);
+
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+  }
+
+  {
+    auto builder = drivetrain_status_sender_.MakeBuilder();
+
+    frc971::control_loops::drivetrain::LocalizerState::Builder
+        localizer_builder = builder.MakeBuilder<
+            frc971::control_loops::drivetrain::LocalizerState>();
+    localizer_builder.add_left_velocity(0.0);
+    localizer_builder.add_right_velocity(0.0);
+    const auto localizer_offset = localizer_builder.Finish();
+
+    DrivetrainStatus::Builder status_builder =
+        builder.MakeBuilder<DrivetrainStatus>();
+
+    // Set the robot up at kShotAngle off from the target, 1m away.
+    status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle));
+    status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle));
+    status_builder.add_theta(0.0);
+    status_builder.add_localizer(localizer_offset);
+
+    ASSERT_TRUE(builder.Send(status_builder.Finish()));
+  }
+
+  // Give it time to stabilize.
+  RunFor(chrono::seconds(1));
+
+  superstructure_status_fetcher_.Fetch();
+  EXPECT_NEAR(kShotAngle, superstructure_status_fetcher_->turret()->position(),
+              1e-4);
+  EXPECT_FLOAT_EQ(kShotAngle,
+                  superstructure_status_fetcher_->aimer()->turret_position());
+  EXPECT_FLOAT_EQ(0,
+                  superstructure_status_fetcher_->aimer()->turret_velocity());
+}
+
+INSTANTIATE_TEST_CASE_P(ShootAnyAlliance, SuperstructureAllianceTest,
+                        ::testing::Values(aos::Alliance::kRed,
+                                          aos::Alliance::kBlue,
+                                          aos::Alliance::kInvalid));
+
 }  // namespace testing
 }  // namespace superstructure
 }  // namespace control_loops
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index e8f7637..81293d3 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 {
@@ -26,6 +29,15 @@
   accelerator_right:FlywheelControllerStatus;
 }
 
+table AimerStatus {
+  // The current goal angle for the turret auto-tracking, in radians.
+  turret_position:double;
+  // The current goal velocity for the turret, in radians / sec.
+  turret_velocity:double;
+  // Whether we are currently aiming for the inner port.
+  aiming_for_inner_port:bool;
+}
+
 table Status {
   // All subsystems know their location.
   zeroed:bool;
@@ -43,6 +55,9 @@
 
   // Status of the control_panel
   control_panel:frc971.control_loops.RelativeEncoderProfiledJointStatus;
+
+  // Status of the vision auto-tracking.
+  aimer:AimerStatus;
 }
 
 root_type Status;
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 894d418..010c6fd 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -30,3 +30,28 @@
         "//frc971/control_loops:state_feedback_loop",
     ],
 )
+
+cc_library(
+    name = "aiming",
+    srcs = ["aiming.cc"],
+    hdrs = ["aiming.h"],
+    deps = [
+        "//aos:flatbuffers",
+        "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops:pose",
+        "//frc971/control_loops:profiled_subsystem_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+        "//y2020:constants",
+        "//y2020/control_loops/drivetrain:drivetrain_base",
+        "//y2020/control_loops/superstructure:superstructure_status_fbs",
+    ],
+)
+
+cc_test(
+    name = "aiming_test",
+    srcs = ["aiming_test.cc"],
+    deps = [
+        ":aiming",
+        "//aos/testing:googletest",
+    ],
+)
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
new file mode 100644
index 0000000..360fd1a
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -0,0 +1,252 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+using frc971::control_loops::Pose;
+
+// Shooting-on-the-fly concept:
+// The current way that we manage shooting-on-the fly endeavors to be reasonably
+// simple, until we get a chance to see how the actual dynamics play out.
+// Essentially, we assume that the robot's velocity will represent a constant
+// offset to the ball's velocity over the entire trajectory to the goal and
+// then offset the target that we are pointing at based on that.
+// Let us assume that, if the robot shoots while not moving, regardless of shot
+// distance, the ball's average speed-over-ground to the target will be a
+// constant s_shot (this implies that if the robot is driving straight towards
+// the target, the actual ball speed-over-ground will be greater than s_shot).
+// We will define things in the robot's coordinate frame. We will be shooting
+// at a target that is at position (target_x, target_y) in the robot frame. The
+// robot is travelling at (v_robot_x, v_robot_y). In order to shoot the ball,
+// we need to generate some virtual target (virtual_x, virtual_y) that we will
+// shoot at as if we were standing still. The total time-of-flight to that
+// target will be t_shot = norm2(virtual_x, virtual_y) / s_shot.
+// we will have virtual_x + v_robot_x * t_shot = target_x, and the same
+// for y. This gives us three equations and three unknowns (virtual_x,
+// virtual_y, and t_shot), and given appropriate assumptions, can be solved
+// analytically. However, doing so is obnoxious and given appropriate functions
+// for t_shot may not be feasible. As such, instead of actually solving the
+// equation analytically, we will use an iterative solution where we maintain
+// a current virtual target estimate. We start with this estimate as if the
+// robot is stationary. We then use this estimate to calculate t_shot, and
+// calculate the next value for the virtual target.
+
+namespace {
+// The overall length and width of the field, in meters.
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+// Height of the center of the port(s) above the ground, in meters.
+constexpr double kPortHeight = 2.494;
+
+// Maximum shot angle at which we will attempt to make the shot into the inner
+// port, in radians. Zero would imply that we could only shoot if we were
+// exactly perpendicular to the target. Larger numbers allow us to aim at the
+// inner port more aggressively, at the risk of being more likely to miss the
+// outer port entirely.
+constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
+
+// Distance (in meters) from the edge of the field to the port.
+constexpr double kEdgeOfFieldToPort = 2.404;
+
+// The amount (in meters) that the inner port is set back from the outer port.
+constexpr double kInnerPortBackset = 0.743;
+
+// Average speed-over-ground of the ball on its way to the target. Our current
+// model assumes constant ball velocity regardless of shot distance.
+// TODO(james): Is this an appropriate model? For the outer port it should be
+// good enough that it doesn't really matter, but for the inner port it may be
+// more appropriate to do something more dynamic--however, it is not yet clear
+// how we would best estimate speed-over-ground given a hood angle + shooter
+// speed. Assuming a constant average speed over the course of the trajectory
+// should be reasonable, since all we are trying to do here is calculate an
+// overall time-of-flight (we don't actually care about the ball speed itself).
+constexpr double kBallSpeedOverGround = 15.0;  // m/s
+
+// Minimum distance that we must be from the inner port in order to attempt the
+// shot--this is to account for the fact that if we are too close to the target,
+// then we won't have a clear shot on the inner port.
+constexpr double kMinimumInnerPortShotDistance = 4.0;
+
+// Amount of buffer, in radians, to leave to help avoid wrapping. I.e., any time
+// that we are in kAvoidEdges mode, we will keep ourselves at least
+// kAntiWrapBuffer radians away from the hardstops.
+constexpr double kAntiWrapBuffer = 0.2;
+
+// If the turret is at zero, then it will be at this angle relative to pointed
+// straight forwards on the robot.
+constexpr double kTurretZeroOffset = M_PI;
+
+constexpr double kTurretRange = constants::Values::kTurretRange().range();
+static_assert((kTurretRange - 2.0 * kAntiWrapBuffer) > 2.0 * M_PI,
+              "kAntiWrap buffer should be small enough that we still have 360 "
+              "degrees of range.");
+
+Pose ReverseSideOfField(Pose target) {
+  *target.mutable_pos() *= -1;
+  target.set_theta(aos::math::NormalizeAngle(target.rel_theta() + M_PI));
+  return target;
+}
+
+flatbuffers::DetachedBuffer MakePrefilledGoal() {
+  flatbuffers::FlatBufferBuilder fbb;
+  fbb.ForceDefaults(true);
+  Aimer::Goal::Builder builder(fbb);
+  builder.add_unsafe_goal(0);
+  builder.add_goal_velocity(0);
+  builder.add_ignore_profile(true);
+  fbb.Finish(builder.Finish());
+  return fbb.Release();
+}
+
+// This implements the iteration in the described shooting-on-the-fly algorithm.
+// robot_pose: Current robot pose.
+// robot_velocity: Current robot velocity, in the absolute field frame.
+// target_pose: Absolute goal Pose.
+// current_virtual_pose: Current estimate of where we want to shoot at.
+Pose IterateVirtualGoal(const Pose &robot_pose,
+                        const Eigen::Vector3d &robot_velocity,
+                        const Pose &target_pose,
+                        const Pose &current_virtual_pose) {
+  const double air_time =
+      current_virtual_pose.Rebase(&robot_pose).xy_norm() / kBallSpeedOverGround;
+  const Eigen::Vector3d virtual_target =
+      target_pose.abs_pos() - air_time * robot_velocity;
+  return Pose(virtual_target, target_pose.abs_theta());
+}
+}  // namespace
+
+Pose InnerPortPose(aos::Alliance alliance) {
+  const Pose target({kFieldLength / 2 + kInnerPortBackset,
+                     -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+                    0.0);
+  if (alliance == aos::Alliance::kRed) {
+    return ReverseSideOfField(target);
+  }
+  return target;
+}
+
+Pose OuterPortPose(aos::Alliance alliance) {
+  Pose target(
+      {kFieldLength / 2, -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+      0.0);
+  if (alliance == aos::Alliance::kRed) {
+    return ReverseSideOfField(target);
+  }
+  return target;
+}
+
+Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+
+void Aimer::Update(const Status *status, aos::Alliance alliance,
+                   WrapMode wrap_mode, ShotMode shot_mode) {
+  const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
+  const Pose inner_port = InnerPortPose(alliance);
+  const Pose outer_port = OuterPortPose(alliance);
+  const Pose robot_pose_from_inner_port = robot_pose.Rebase(&inner_port);
+
+  // TODO(james): This code should probably just be in the localizer and have
+  // xdot/ydot get populated in the status message directly... that way we don't
+  // keep duplicating this math.
+  // Also, this doesn't currently take into account the lateral velocity of the
+  // robot. All of this would be helped by just doing this work in the Localizer
+  // itself.
+  const Eigen::Vector2d linear_angular =
+      drivetrain::GetDrivetrainConfig().Tlr_to_la() *
+      Eigen::Vector2d(status->localizer()->left_velocity(),
+                      status->localizer()->right_velocity());
+  const double xdot = linear_angular(0) * std::cos(status->theta());
+  const double ydot = linear_angular(0) * std::sin(status->theta());
+
+  const double inner_port_angle = robot_pose_from_inner_port.heading();
+  const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
+  aiming_for_inner_port_ =
+      (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
+      (inner_port_distance > kMinimumInnerPortShotDistance);
+
+  // This code manages compensating the goal turret heading for the robot's
+  // current velocity, to allow for shooting on-the-fly.
+  // This works by solving for the correct turret angle numerically, since while
+  // we technically could do it analytically, doing so would both make it hard
+  // to make small changes (since it would force us to redo the math) and be
+  // error-prone since it'd be easy to make typos or other minor math errors.
+  Pose virtual_goal;
+  {
+    const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
+    virtual_goal = goal;
+    if (shot_mode == ShotMode::kShootOnTheFly) {
+      for (int ii = 0; ii < 3; ++ii) {
+        virtual_goal =
+            IterateVirtualGoal(robot_pose, {xdot, ydot, 0}, goal, virtual_goal);
+      }
+      VLOG(1) << "Shooting-on-the-fly target position: "
+              << virtual_goal.abs_pos().transpose();
+    }
+    virtual_goal = virtual_goal.Rebase(&robot_pose);
+  }
+
+  const double heading_to_goal = virtual_goal.heading();
+  CHECK(status->has_localizer());
+  distance_ = virtual_goal.xy_norm();
+
+  // The following code all works to calculate what the rate of turn of the
+  // turret should be. The code only accounts for the rate of turn if we are
+  // aiming at a static target, which should be close enough to correct that it
+  // doesn't matter that it fails to account for the
+  // shooting-on-the-fly compensation.
+  const double rel_x = virtual_goal.rel_pos().x();
+  const double rel_y = virtual_goal.rel_pos().y();
+  const double squared_norm = rel_x * rel_x + rel_y * rel_y;
+
+  // If squared_norm gets to be too close to zero, just zero out the relevant
+  // term to prevent NaNs. Note that this doesn't address the chattering that
+  // would likely occur if we were to get excessively close to the target.
+  // Note that x and y terms are swapped relative to what you would normally see
+  // in the derivative of atan because xdot and ydot are the derivatives of
+  // robot_pos and we are working with the atan of (target_pos - robot_pos).
+  const double atan_diff = (squared_norm < 1e-3)
+                               ? 0.0
+                               : (rel_y * xdot - rel_x * ydot) / squared_norm;
+  // heading = atan2(relative_y, relative_x) - robot_theta
+  // dheading / dt = (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
+  const double dheading_dt = atan_diff - linear_angular(1);
+
+  double range = kTurretRange;
+  if (wrap_mode == WrapMode::kAvoidEdges) {
+    range -= 2.0 * kAntiWrapBuffer;
+  }
+  // Calculate a goal turret heading such that it is within +/- pi of the
+  // current position (i.e., a goal that would minimize the amount the turret
+  // would have to travel).
+  // We then check if this goal would bring us out of range of the valid angles,
+  // and if it would, we reset to be within +/- pi of zero.
+  double turret_heading =
+      goal_.message().unsafe_goal() +
+      aos::math::NormalizeAngle(heading_to_goal - kTurretZeroOffset -
+                                goal_.message().unsafe_goal());
+  if (std::abs(turret_heading - constants::Values::kTurretRange().middle()) >
+      range / 2.0) {
+    turret_heading = aos::math::NormalizeAngle(turret_heading);
+  }
+
+  goal_.mutable_message()->mutate_unsafe_goal(turret_heading);
+  goal_.mutable_message()->mutate_goal_velocity(dheading_dt);
+}
+
+flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
+    flatbuffers::FlatBufferBuilder *fbb) const {
+  AimerStatus::Builder builder(*fbb);
+  builder.add_turret_position(goal_.message().unsafe_goal());
+  builder.add_turret_velocity(goal_.message().goal_velocity());
+  builder.add_aiming_for_inner_port(aiming_for_inner_port_);
+  return builder.Finish();
+}
+
+}  // namespace turret
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2020
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
new file mode 100644
index 0000000..3b3071e
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -0,0 +1,74 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+
+#include "aos/flatbuffers.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+// Returns the port that we want to score on given our current alliance. The yaw
+// of the port will be such that the positive x axis points out the back of the
+// target.
+frc971::control_loops::Pose InnerPortPose(aos::Alliance alliance);
+frc971::control_loops::Pose OuterPortPose(aos::Alliance alliance);
+
+// This class manages taking in drivetrain status messages and generating turret
+// goals so that it gets aimed at the goal.
+class Aimer {
+ public:
+  typedef frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+      Goal;
+  typedef frc971::control_loops::drivetrain::Status Status;
+  // Mode to run the aimer in, to control how we manage wrapping the turret
+  // angle.
+  enum class WrapMode {
+    // Keep the turret as far away from the edges of the range of motion as
+    // reasonable, to minimize the odds that we will hit the hardstops once we
+    // start shooting.
+    kAvoidEdges,
+    // Do everything reasonable to avoid having to wrap the shooter--set this
+    // while shooting so that we don't randomly spin the shooter 360 while
+    // shooting.
+    kAvoidWrapping,
+  };
+
+  // Control modes for managing how we manage shooting on the fly.
+  enum class ShotMode {
+    // Don't do any shooting-on-the-fly compensation--just point straight at the
+    // target. Primarily used in tests.
+    kStatic,
+    // Do do shooting-on-the-fly compensation.
+    kShootOnTheFly,
+  };
+
+  Aimer();
+
+  void Update(const Status *status, aos::Alliance alliance, WrapMode wrap_mode,
+              ShotMode shot_mode);
+
+  const Goal *TurretGoal() const { return &goal_.message(); }
+
+  // Returns the distance to the goal, in meters.
+  double DistanceToGoal() const { return distance_; }
+
+  flatbuffers::Offset<AimerStatus> PopulateStatus(
+      flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+  aos::FlatbufferDetachedBuffer<Goal> goal_;
+  bool aiming_for_inner_port_ = false;
+  double distance_ = 0.0;
+};
+
+}  // namespace turret
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2020
+#endif  // y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
new file mode 100644
index 0000000..cb95b56
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -0,0 +1,249 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "frc971/control_loops/pose.h"
+#include "gtest/gtest.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+namespace testing {
+
+using frc971::control_loops::Pose;
+
+class AimerTest : public ::testing::Test {
+ public:
+  typedef Aimer::Goal Goal;
+  typedef Aimer::Status Status;
+  struct StatusData {
+    double x;
+    double y;
+    double theta;
+    double linear;
+    double angular;
+  };
+  aos::FlatbufferDetachedBuffer<Status> MakeStatus(const StatusData &data) {
+    flatbuffers::FlatBufferBuilder fbb;
+    frc971::control_loops::drivetrain::LocalizerState::Builder state_builder(
+        fbb);
+    state_builder.add_left_velocity(
+        data.linear -
+        data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+    state_builder.add_right_velocity(
+        data.linear +
+        data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+    const auto state_offset = state_builder.Finish();
+    Status::Builder builder(fbb);
+    builder.add_x(data.x);
+    builder.add_y(data.y);
+    builder.add_theta(data.theta);
+    builder.add_localizer(state_offset);
+    fbb.Finish(builder.Finish());
+    return fbb.Release();
+  }
+
+  const Goal *Update(
+      const StatusData &data, aos::Alliance alliance = aos::Alliance::kBlue,
+      Aimer::WrapMode wrap_mode = Aimer::WrapMode::kAvoidEdges,
+      Aimer::ShotMode shot_mode = Aimer::ShotMode::kShootOnTheFly) {
+    const auto buffer = MakeStatus(data);
+    aimer_.Update(&buffer.message(), alliance, wrap_mode, shot_mode);
+    const Goal *goal = aimer_.TurretGoal();
+    EXPECT_TRUE(goal->ignore_profile());
+    return goal;
+  }
+
+ protected:
+  Aimer aimer_;
+};
+
+TEST_F(AimerTest, StandingStill) {
+  const Pose target = OuterPortPose(aos::Alliance::kBlue);
+  const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+                             .y = target.abs_pos().y() + 0.0,
+                             .theta = 0.0,
+                             .linear = 0.0,
+                             .angular = 0.0});
+  EXPECT_EQ(0.0, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+  goal = Update({.x = target.abs_pos().x() + 1.0,
+                 .y = target.abs_pos().y() + 0.0,
+                 .theta = 1.0,
+                 .linear = 0.0,
+                 .angular = 0.0});
+  EXPECT_EQ(-1.0, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  goal = Update({.x = target.abs_pos().x() + 1.0,
+                 .y = target.abs_pos().y() + 0.0,
+                 .theta = -1.0,
+                 .linear = 0.0,
+                 .angular = 0.0});
+  EXPECT_EQ(1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+  // Test that we handle the case that where we are right on top of the target.
+  goal = Update({.x = target.abs_pos().x() + 0.0,
+                 .y = target.abs_pos().y() + 0.0,
+                 .theta = 0.0,
+                 .linear = 0.0,
+                 .angular = 0.0});
+  EXPECT_EQ(M_PI, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  EXPECT_EQ(0.0, aimer_.DistanceToGoal());
+}
+
+TEST_F(AimerTest, SpinningRobot) {
+  const Pose target = OuterPortPose(aos::Alliance::kBlue);
+  const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+                             .y = target.abs_pos().y() + 0.0,
+                             .theta = 0.0,
+                             .linear = 0.0,
+                             .angular = 1.0});
+  EXPECT_EQ(0.0, goal->unsafe_goal());
+  EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+}
+
+// Tests that when we drive straight away from the target we don't have to spin
+// the turret.
+TEST_F(AimerTest, DrivingAwayFromTarget) {
+  const Pose target = OuterPortPose(aos::Alliance::kBlue);
+  // To keep the test simple, disable shooting on the fly so that the
+  // goal distance comes out in an easy to calculate number.
+  const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+                             .y = target.abs_pos().y() + 0.0,
+                             .theta = 0.0,
+                             .linear = 1.0,
+                             .angular = 0.0},
+                            aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+                            Aimer::ShotMode::kStatic);
+  EXPECT_EQ(0.0, goal->unsafe_goal());
+  EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+  EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+  // Next, try with shooting-on-the-fly enabled--because we are driving straight
+  // towards the target, only the goal distance should be impacted.
+  goal = Update({.x = target.abs_pos().x() + 1.0,
+                 .y = target.abs_pos().y() + 0.0,
+                 .theta = 0.0,
+                 .linear = 1.0,
+                 .angular = 0.0},
+                aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+                Aimer::ShotMode::kShootOnTheFly);
+  EXPECT_EQ(0.0, goal->unsafe_goal());
+  EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+  EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+  EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Tests that when we drive perpendicular to the target, we do have to spin.
+TEST_F(AimerTest, DrivingLateralToTarget) {
+  const Pose target = OuterPortPose(aos::Alliance::kBlue);
+  // To keep the test simple, disable shooting on the fly so that the
+  // goal_velocity comes out in an easy to calculate number.
+  const Goal *goal = Update({.x = target.abs_pos().x() + 0.0,
+                             .y = target.abs_pos().y() + 1.0,
+                             .theta = 0.0,
+                             .linear = 1.0,
+                             .angular = 0.0},
+                            aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+                            Aimer::ShotMode::kStatic);
+  EXPECT_EQ(M_PI_2, goal->unsafe_goal());
+  EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+  EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+  // Next, test with shooting-on-the-fly enabled, The goal numbers should all be
+  // slightly offset due to the robot velocity.
+  goal = Update({.x = target.abs_pos().x() + 0.0,
+                 .y = target.abs_pos().y() + 1.0,
+                 .theta = 0.0,
+                 .linear = 1.0,
+                 .angular = 0.0},
+                aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+                Aimer::ShotMode::kShootOnTheFly);
+  // Confirm that the turret heading goal is less then -pi / 2, but not by too
+  // much.
+  EXPECT_GT(M_PI_2 - 0.001, goal->unsafe_goal());
+  EXPECT_LT(M_PI_2 - 0.1, goal->unsafe_goal());
+  // Similarly, the turret velocity goal should be a bit greater than -1.0,
+  // since the turret is no longer at exactly a right angle.
+  EXPECT_LT(-1.0, goal->goal_velocity());
+  EXPECT_GT(-0.95, goal->goal_velocity());
+  // And the distance to the goal should be a bit greater than 1.0.
+  EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+  EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Confirms that we will indeed shoot at the inner port when we have a good shot
+// angle on it.
+TEST_F(AimerTest, InnerPort) {
+  const Pose target = InnerPortPose(aos::Alliance::kRed);
+  const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+                             .y = target.abs_pos().y() + 0.0,
+                             .theta = 0.0,
+                             .linear = 0.0,
+                             .angular = 0.0},
+                            aos::Alliance::kRed);
+  EXPECT_EQ(0.0, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that when we move the turret heading so that it would be entirely
+// out of the normal range of motion that we send a valid (in-range) goal.
+TEST_F(AimerTest, WrapWhenOutOfRange) {
+  // Start ourselves needing a turret angle of M_PI.
+  const Pose target = OuterPortPose(aos::Alliance::kBlue);
+  StatusData status{.x = target.abs_pos().x() + 1.0,
+                    .y = target.abs_pos().y() + 0.0,
+                    .theta = 0.0,
+                    .linear = 0.0,
+                    .angular = 0.0};
+  const Goal *goal = Update(status);
+  EXPECT_EQ(0.0, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  // Move the robot a small amount--we should go past pi and not wrap yet.
+  status.theta = -0.1;
+  goal = Update(status);
+  EXPECT_FLOAT_EQ(0.1, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  // Move the robot so that, if we had no hard-stops, we would go past it.
+  status.theta = -2.0;
+  goal = Update(status);
+  EXPECT_FLOAT_EQ(2.0, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that the avoid edges turret mode doesn't let us get all the way to
+// the turret hard-stops but that the avoid wrapping mode does.
+TEST_F(AimerTest, WrappingModes) {
+  // Start ourselves needing a turret angle of M_PI.
+  const Pose target = OuterPortPose(aos::Alliance::kBlue);
+  StatusData status{.x = target.abs_pos().x() - 1.0,
+                    .y = target.abs_pos().y() + 0.0,
+                    .theta = 0.0,
+                    .linear = 0.0,
+                    .angular = 0.0};
+  const Goal *goal =
+      Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+  EXPECT_EQ(M_PI, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  constexpr double kUpperLimit = constants::Values::kTurretRange().upper;
+  // Move the robot to the upper limit with AvoidWrapping set--we should be at
+  // the upper limit and not wrapped.
+  status.theta = goal->unsafe_goal() - kUpperLimit;
+  goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+  EXPECT_FLOAT_EQ(kUpperLimit, goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+  // Enter kAvoidEdges mode--we should wrap around.
+  goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges);
+  // confirm that this test is actually testing something...
+  ASSERT_NE(aos::math::NormalizeAngle(kUpperLimit), kUpperLimit);
+  EXPECT_FLOAT_EQ(aos::math::NormalizeAngle(kUpperLimit), goal->unsafe_goal());
+  EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+}  // namespace testing
+}  // namespace turret
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2020
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 7910859..2049e83 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -13,10 +13,11 @@
 #include "aos/network/team_number.h"
 #include "aos/util/log_interval.h"
 #include "frc971/autonomous/base_autonomous_actor.h"
-#include "y2020/control_loops/drivetrain/drivetrain_base.h"
 #include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
 #include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/setpoint_generated.h"
 
 using aos::input::driver_station::ButtonLocation;
 using aos::input::driver_station::ControlBit;
@@ -40,9 +41,12 @@
 const ButtonLocation kTurret(3, 15);
 const ButtonLocation kHood(3, 3);
 const ButtonLocation kShootSlow(4, 2);
+const ButtonLocation kFeed(4, 1);
 const ButtonLocation kIntakeExtend(3, 9);
 const ButtonLocation kIntakeIn(4, 4);
-const ButtonLocation kSpit(4, 5);
+const ButtonLocation kSpit(4, 3);
+
+const ButtonLocation kWinch(3, 14);
 
 class Reader : public ::aos::input::ActionJoystickInput {
  public:
@@ -54,8 +58,9 @@
         superstructure_goal_sender_(
             event_loop->MakeSender<superstructure::Goal>("/superstructure")),
         superstructure_status_fetcher_(
-            event_loop->MakeFetcher<superstructure::Status>(
-                "/superstructure")) {}
+            event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+        setpoint_fetcher_(event_loop->MakeFetcher<y2020::joysticks::Setpoint>(
+            "/superstructure")) {}
 
   void AutoEnded() override {
     AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
@@ -68,31 +73,54 @@
       return;
     }
 
+    setpoint_fetcher_.Fetch();
+
     double hood_pos = constants::Values::kHoodRange().middle();
     double intake_pos = -0.89;
     double turret_pos = 0.0;
     float roller_speed = 0.0f;
     double accelerator_speed = 0.0;
     double finisher_speed = 0.0;
+    double climber_speed = 0.0;
 
     if (data.IsPressed(kTurret)) {
       turret_pos = 3.5;
     }
 
     if (data.IsPressed(kHood)) {
-      hood_pos = 0.05;
+      hood_pos = 0.45;
+    } else {
+      if (setpoint_fetcher_.get()) {
+        hood_pos = setpoint_fetcher_->hood();
+      } else {
+        hood_pos = 0.58;
+      }
     }
 
     if (data.IsPressed(kShootFast)) {
-      accelerator_speed = 600.0;
-      finisher_speed = 200.0;
+      if (setpoint_fetcher_.get()) {
+        accelerator_speed = setpoint_fetcher_->accelerator();
+        finisher_speed = setpoint_fetcher_->finisher();
+      } else {
+        accelerator_speed = 250.0;
+        finisher_speed = 500.0;
+      }
     } else if (data.IsPressed(kShootSlow)) {
-      accelerator_speed = 30.0;
-      finisher_speed = 30.0;
+      accelerator_speed = 180.0;
+      finisher_speed = 375.0;
     }
 
     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)) {
     }
 
     if (data.IsPressed(kIntakeIn)) {
@@ -101,6 +129,10 @@
       roller_speed = -6.0f;
     }
 
+    if (data.IsPressed(kWinch)) {
+      climber_speed = 12.0f;
+    }
+
     auto builder = superstructure_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
@@ -108,7 +140,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(
@@ -132,6 +164,8 @@
       superstructure_goal_builder.add_turret(turret_offset);
       superstructure_goal_builder.add_roller_voltage(roller_speed);
       superstructure_goal_builder.add_shooter(shooter_offset);
+      superstructure_goal_builder.add_shooting(data.IsPressed(kFeed));
+      superstructure_goal_builder.add_climber_voltage(climber_speed);
 
       if (!builder.Send(superstructure_goal_builder.Finish())) {
         AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
@@ -143,6 +177,8 @@
   ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
 
   ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+
+  ::aos::Fetcher<y2020::joysticks::Setpoint> setpoint_fetcher_;
 };
 
 }  // namespace joysticks
diff --git a/y2020/setpoint.fbs b/y2020/setpoint.fbs
new file mode 100644
index 0000000..5f1af72
--- /dev/null
+++ b/y2020/setpoint.fbs
@@ -0,0 +1,11 @@
+namespace y2020.joysticks;
+
+table Setpoint {
+  accelerator:double;
+
+  finisher:double;
+
+  hood:double;
+}
+
+root_type Setpoint;
diff --git a/y2020/setpoint_setter.cc b/y2020/setpoint_setter.cc
new file mode 100644
index 0000000..1cbc518
--- /dev/null
+++ b/y2020/setpoint_setter.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "gflags/gflags.h"
+#include "y2020/setpoint_generated.h"
+
+DEFINE_double(accelerator, 250.0, "Accelerator speed");
+DEFINE_double(finisher, 500.0, "Finsher speed");
+DEFINE_double(hood, 0.45, "Hood setpoint");
+
+int main(int argc, char ** argv) {
+  aos::InitGoogle(&argc, &argv);
+  aos::InitNRT();
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig("config.json");
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  ::aos::Sender<y2020::joysticks::Setpoint> setpoint_sender =
+      event_loop.MakeSender<y2020::joysticks::Setpoint>("/superstructure");
+
+  aos::Sender<y2020::joysticks::Setpoint>::Builder builder =
+      setpoint_sender.MakeBuilder();
+
+  y2020::joysticks::Setpoint::Builder setpoint_builder =
+      builder.MakeBuilder<y2020::joysticks::Setpoint>();
+
+  setpoint_builder.add_accelerator(FLAGS_accelerator);
+  setpoint_builder.add_finisher(FLAGS_finisher);
+  setpoint_builder.add_hood(FLAGS_hood);
+  builder.Send(setpoint_builder.Finish());
+
+  aos::Cleanup();
+}
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index fb7b113..3baa61b 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -16,6 +16,7 @@
     hdrs = [
         "v4l2_reader.h",
     ],
+    visibility = ["//y2020:__subpackages__"],
     deps = [
         ":vision_fbs",
         "//aos/events:event_loop",
@@ -30,10 +31,14 @@
     srcs = [
         "camera_reader.cc",
     ],
+    data = [
+        "//y2020:config.json",
+    ],
     restricted_to = [
         "//tools:k8",
         "//tools:armhf-debian",
     ],
+    visibility = ["//y2020:__subpackages__"],
     deps = [
         ":v4l2_reader",
         ":vision_fbs",
@@ -42,10 +47,10 @@
         "//aos/events:shm_event_loop",
         "//aos/network:team_number",
         "//third_party:opencv",
-        "//y2020/vision/sift:demo_sift",
         "//y2020/vision/sift:sift971",
         "//y2020/vision/sift:sift_fbs",
         "//y2020/vision/sift:sift_training_fbs",
+        "//y2020/vision/tools/python_code:sift_training_data",
     ],
 )
 
@@ -54,3 +59,46 @@
     srcs = ["vision.fbs"],
     visibility = ["//y2020:__subpackages__"],
 )
+
+cc_binary(
+    name = "viewer",
+    srcs = [
+        "viewer.cc",
+    ],
+    data = [
+        "//y2020:config.json",
+    ],
+    restricted_to = [
+        "//tools:k8",
+        "//tools:armhf-debian",
+    ],
+    visibility = ["//y2020:__subpackages__"],
+    deps = [
+        ":vision_fbs",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//third_party:opencv",
+    ],
+)
+
+cc_binary(
+    name = "viewer_replay",
+    srcs = [
+        "viewer_replay.cc",
+    ],
+    data = [
+        "//y2020:config.json",
+    ],
+    restricted_to = [
+        "//tools:k8",
+        "//tools:armhf-debian",
+    ],
+    visibility = ["//y2020:__subpackages__"],
+    deps = [
+        ":vision_fbs",
+        "//aos:init",
+        "//aos/events:simulated_event_loop",
+        "//aos/events/logging:logger",
+        "//third_party:opencv",
+    ],
+)
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 06e4077..f38a40e 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -7,13 +7,18 @@
 #include "aos/init.h"
 #include "aos/network/team_number.h"
 
-#include "y2020/vision/sift/demo_sift.h"
 #include "y2020/vision/sift/sift971.h"
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
 #include "y2020/vision/v4l2_reader.h"
 #include "y2020/vision/vision_generated.h"
 
+// config used to allow running camera_reader independently.  E.g.,
+// bazel run //y2020/vision:camera_reader -- --config y2020/config.json
+//   --override_hostname pi-7971-1  --ignore_timestamps true
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+
 namespace frc971 {
 namespace vision {
 namespace {
@@ -31,6 +36,8 @@
         image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
         result_sender_(
             event_loop->MakeSender<sift::ImageMatchResult>("/camera")),
+        detailed_result_sender_(
+            event_loop->MakeSender<sift::ImageMatchResult>("/camera/detailed")),
         read_image_timer_(event_loop->AddTimer([this]() {
           ReadImage();
           read_image_timer_->Setup(event_loop_->monotonic_now());
@@ -63,6 +70,33 @@
                const std::vector<cv::KeyPoint> &keypoints,
                const cv::Mat &descriptors);
 
+  void SendImageMatchResult(const CameraImage &image,
+                            const std::vector<cv::KeyPoint> &keypoints,
+                            const cv::Mat &descriptors,
+                            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);
+
+  // Returns the 2D (image) location for the specified training feature.
+  cv::Point2f Training2dPoint(int training_image_index,
+                              int feature_index) const {
+    const float x = training_data_->images()
+                        ->Get(training_image_index)
+                        ->features()
+                        ->Get(feature_index)
+                        ->x();
+    const float y = training_data_->images()
+                        ->Get(training_image_index)
+                        ->features()
+                        ->Get(feature_index)
+                        ->y();
+    return cv::Point2f(x, y);
+  }
+
   // Returns the 3D location for the specified training feature.
   cv::Point3f Training3dPoint(int training_image_index,
                               int feature_index) const {
@@ -81,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();
   }
@@ -93,6 +138,14 @@
     return result;
   }
 
+  cv::Mat CameraDistCoeffs() const {
+    const cv::Mat result(5, 1, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             camera_calibration_->dist_coeffs()->data())));
+    CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
+    return result;
+  }
+
   aos::EventLoop *const event_loop_;
   const sift::TrainingData *const training_data_;
   const sift::CameraCalibration *const camera_calibration_;
@@ -100,6 +153,7 @@
   cv::FlannBasedMatcher *const matcher_;
   aos::Sender<CameraImage> image_sender_;
   aos::Sender<sift::ImageMatchResult> result_sender_;
+  aos::Sender<sift::ImageMatchResult> detailed_result_sender_;
   // We schedule this immediately to read an image. Having it on a timer means
   // other things can run on the event loop in between.
   aos::TimerHandler *const read_image_timer_;
@@ -128,37 +182,103 @@
 void CameraReader::CopyTrainingFeatures() {
   for (const sift::TrainingImage *training_image : *training_data_->images()) {
     cv::Mat features(training_image->features()->size(), 128, CV_32F);
-    for (size_t i = 0; i <  training_image->features()->size(); ++i) {
+    for (size_t i = 0; i < training_image->features()->size(); ++i) {
       const sift::Feature *feature_table = training_image->features()->Get(i);
 
       // We don't need this information right now, but make sure it's here to
       // avoid crashes that only occur when specific features are matched.
       CHECK(feature_table->has_field_location());
 
-      const flatbuffers::Vector<float> *const descriptor =
+      const flatbuffers::Vector<uint8_t> *const descriptor =
           feature_table->descriptor();
       CHECK_EQ(descriptor->size(), 128u) << ": Unsupported feature size";
-      cv::Mat(1, descriptor->size(), CV_32F,
-              const_cast<void *>(static_cast<const void *>(descriptor->data())))
-          .copyTo(features(cv::Range(i, i + 1), cv::Range(0, 128)));
+      const auto in_mat = cv::Mat(
+          1, descriptor->size(), CV_8U,
+          const_cast<void *>(static_cast<const void *>(descriptor->data())));
+      const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
+      in_mat.convertTo(out_mat, CV_32F);
     }
     matcher_->add(features);
   }
 }
 
-void CameraReader::ProcessImage(const CameraImage &image) {
-  // Be ready to pack the results up and send them out. We can pack things into
-  // this memory as we go to allow reusing temporaries better.
-  auto builder = result_sender_.MakeBuilder();
+void CameraReader::SendImageMatchResult(
+    const CameraImage &image, const std::vector<cv::KeyPoint> &keypoints,
+    const cv::Mat &descriptors,
+    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 =
       aos::CopyFlatBuffer(camera_calibration_, builder.fbb());
 
+  flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<sift::Feature>>>
+      features_offset;
+  flatbuffers::Offset<
+      flatbuffers::Vector<flatbuffers::Offset<sift::ImageMatch>>>
+      image_matches_offset;
+  if (send_details) {
+    features_offset = PackFeatures(builder.fbb(), keypoints, descriptors);
+    image_matches_offset = PackImageMatches(builder.fbb(), matches);
+  }
+
+  std::vector<flatbuffers::Offset<sift::CameraPose>> camera_poses;
+
+  CHECK_EQ(camera_target_list.size(), field_camera_list.size());
+  for (size_t i = 0; i < camera_target_list.size(); ++i) {
+    cv::Mat camera_target = camera_target_list[i];
+    CHECK(camera_target.isContinuous());
+    const auto data_offset = builder.fbb()->CreateVector<float>(
+        reinterpret_cast<float *>(camera_target.data), camera_target.total());
+    const flatbuffers::Offset<sift::TransformationMatrix> transform_offset =
+        sift::CreateTransformationMatrix(*builder.fbb(), data_offset);
+
+    cv::Mat field_camera = field_camera_list[i];
+    CHECK(field_camera.isContinuous());
+    const auto fc_data_offset = builder.fbb()->CreateVector<float>(
+        reinterpret_cast<float *>(field_camera.data), field_camera.total());
+    const flatbuffers::Offset<sift::TransformationMatrix> fc_transform_offset =
+        sift::CreateTransformationMatrix(*builder.fbb(), fc_data_offset);
+
+    const flatbuffers::Offset<sift::TransformationMatrix>
+        field_to_target_offset =
+            aos::CopyFlatBuffer(FieldToTarget(i), builder.fbb());
+
+    sift::CameraPose::Builder pose_builder(*builder.fbb());
+    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);
+
+  sift::ImageMatchResult::Builder result_builder(*builder.fbb());
+  result_builder.add_camera_poses(camera_poses_offset);
+  if (send_details) {
+    result_builder.add_image_matches(image_matches_offset);
+    result_builder.add_features(features_offset);
+  }
+  result_builder.add_image_monotonic_timestamp_ns(
+      image.monotonic_timestamp_ns());
+  result_builder.add_camera_calibration(camera_calibration_offset);
+
+  // TODO<Jim>: Need to add target point computed from matches and
+  // mapped by homography
+  builder.Send(result_builder.Finish());
+}
+
+void CameraReader::ProcessImage(const CameraImage &image) {
   // First, we need to extract the brightness information. This can't really be
   // fused into the beginning of the SIFT algorithm because the algorithm needs
   // to look at the base image directly. It also only takes 2ms on our images.
   // This is converting from YUYV to a grayscale image.
-  cv::Mat image_mat(
-      image.rows(), image.cols(), CV_8U);
+  cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
   CHECK(image_mat.isContinuous());
   const int number_pixels = image.rows() * image.cols();
   for (int i = 0; i < number_pixels; ++i) {
@@ -170,18 +290,17 @@
   std::vector<cv::KeyPoint> keypoints;
   cv::Mat descriptors;
   sift_->detectAndCompute(image_mat, cv::noArray(), keypoints, descriptors);
-  const auto features_offset =
-      PackFeatures(builder.fbb(), keypoints, descriptors);
 
   // Then, match those features against our training data.
   std::vector<std::vector<cv::DMatch>> matches;
   matcher_->knnMatch(/* queryDescriptors */ descriptors, matches, /* k */ 2);
-  const auto image_matches_offset = PackImageMatches(builder.fbb(), matches);
 
   struct PerImageMatches {
     std::vector<const std::vector<cv::DMatch> *> matches;
     std::vector<cv::Point3f> training_points_3d;
     std::vector<cv::Point2f> query_points;
+    std::vector<cv::Point2f> training_points;
+    cv::Mat homography;
   };
   std::vector<PerImageMatches> per_image_matches(number_training_images());
 
@@ -204,6 +323,8 @@
     CHECK_LT(training_image, static_cast<int>(per_image_matches.size()));
     PerImageMatches *const per_image = &per_image_matches[training_image];
     per_image->matches.push_back(&match);
+    per_image->training_points.push_back(
+        Training2dPoint(training_image, match[0].trainIdx));
     per_image->training_points_3d.push_back(
         Training3dPoint(training_image, match[0].trainIdx));
 
@@ -213,47 +334,170 @@
 
   // The minimum number of matches in a training image for us to use it.
   static constexpr int kMinimumMatchCount = 10;
+  std::vector<cv::Mat> camera_target_list;
+  std::vector<cv::Mat> field_camera_list;
 
-  std::vector<flatbuffers::Offset<sift::CameraPose>> camera_poses;
+  // 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];
+
+    VLOG(2) << "Number of matches to start: " << per_image_matches.size()
+            << "\n";
+    // If we don't have enough matches to start, skip this set of matches
     if (per_image.matches.size() < kMinimumMatchCount) {
       continue;
     }
 
-    cv::Mat R_camera_target, T_camera_target;
-    cv::solvePnPRansac(per_image.training_points_3d, per_image.query_points,
-                       CameraIntrinsics(), cv::noArray(), R_camera_target,
-                       T_camera_target);
+    // Use homography to determine which matches make sense physically
+    cv::Mat mask;
+    cv::Mat homography =
+        cv::findHomography(per_image.training_points, per_image.query_points,
+                           CV_RANSAC, 3.0, mask);
 
-    sift::CameraPose::Builder pose_builder(*builder.fbb());
-    {
-    CHECK_EQ(cv::Size(3, 3), R_camera_target.size());
-    CHECK_EQ(cv::Size(3, 1), T_camera_target.size());
-    cv::Mat camera_target = cv::Mat::zeros(4, 4, CV_32F);
-    R_camera_target.copyTo(camera_target(cv::Range(0, 3), cv::Range(0, 3)));
-    T_camera_target.copyTo(camera_target(cv::Range(3, 4), cv::Range(0, 3)));
-    camera_target.at<float>(3, 3) = 1;
-    CHECK(camera_target.isContinuous());
-    const auto data_offset = builder.fbb()->CreateVector<float>(
-        reinterpret_cast<float *>(camera_target.data), camera_target.total());
-    pose_builder.add_camera_to_target(
-        sift::CreateTransformationMatrix(*builder.fbb(), data_offset));
+    // If mask doesn't have enough leftover matches, skip these matches
+    if (cv::countNonZero(mask) < kMinimumMatchCount) {
+      continue;
     }
-    pose_builder.add_field_to_target(
-        aos::CopyFlatBuffer(FieldToTarget(i), builder.fbb()));
-    camera_poses.emplace_back(pose_builder.Finish());
-  }
-  const auto camera_poses_offset = builder.fbb()->CreateVector(camera_poses);
 
-  sift::ImageMatchResult::Builder result_builder(*builder.fbb());
-  result_builder.add_image_matches(image_matches_offset);
-  result_builder.add_camera_poses(camera_poses_offset);
-  result_builder.add_features(features_offset);
-  result_builder.add_image_monotonic_timestamp_ns(
-      image.monotonic_timestamp_ns());
-  result_builder.add_camera_calibration(camera_calibration_offset);
-  builder.Send(result_builder.Finish());
+    VLOG(2) << "Number of matches after homography: " << cv::countNonZero(mask)
+            << "\n";
+
+    // 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);
+    for (size_t j = 0; j < per_image.matches.size(); j++) {
+      // Skip if we masked out by homography
+      if (mask.at<uchar>(0, j) != 1) {
+        continue;
+      }
+
+      // Add this to our collection of all matches that passed our criteria
+      all_good_matches.push_back(
+          static_cast<std::vector<cv::DMatch>>(*per_image.matches[j]));
+
+      // Fill out the data for matches per image that made it past
+      // 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]);
+    }
+
+    // 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 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;
+    // Tranform from camera to field origin (global) reference frame
+    cv::Mat R_camera_field_vec, R_camera_field, T_camera_field;
+    // Inverse of camera to field-- defines location of camera in
+    // global (field) reference frame
+    cv::Mat R_field_camera_vec, R_field_camera, T_field_camera;
+
+    // Compute the pose of the camera (global origin relative to camera)
+    cv::solvePnPRansac(per_image_good_match.training_points_3d,
+                       per_image_good_match.query_points, CameraIntrinsics(),
+                       CameraDistCoeffs(), R_camera_field_vec, T_camera_field);
+    CHECK_EQ(cv::Size(1, 3), T_camera_field.size());
+
+    // Convert to float32's (from float64) to be compatible with the rest
+    R_camera_field_vec.convertTo(R_camera_field_vec, CV_32F);
+    T_camera_field.convertTo(T_camera_field, CV_32F);
+
+    // Get matrix version of R_camera_field
+    cv::Rodrigues(R_camera_field_vec, R_camera_field);
+    CHECK_EQ(cv::Size(3, 3), R_camera_field.size());
+
+    // Compute H_field_camera = H_camera_field^-1
+    R_field_camera = R_camera_field.t();
+    T_field_camera = -R_field_camera * (T_camera_field);
+
+    // Extract the field_target transformation
+    const cv::Mat H_field_target(4, 4, CV_32F,
+                                 const_cast<void *>(static_cast<const void *>(
+                                     FieldToTarget(i)->data()->data())));
+
+    const cv::Mat R_field_target =
+        H_field_target(cv::Range(0, 3), cv::Range(0, 3));
+    const cv::Mat T_field_target =
+        H_field_target(cv::Range(0, 3), cv::Range(3, 4));
+
+    // Use it to get the relative pose from camera to target
+    R_camera_target = R_camera_field * (R_field_target);
+    T_camera_target = R_camera_field * (T_field_target) + T_camera_field;
+
+    // Set H_camera_target
+    {
+      CHECK_EQ(cv::Size(3, 3), R_camera_target.size());
+      CHECK_EQ(cv::Size(1, 3), T_camera_target.size());
+      cv::Mat H_camera_target = cv::Mat::zeros(4, 4, CV_32F);
+      R_camera_target.copyTo(H_camera_target(cv::Range(0, 3), cv::Range(0, 3)));
+      T_camera_target.copyTo(H_camera_target(cv::Range(0, 3), cv::Range(3, 4)));
+      H_camera_target.at<float>(3, 3) = 1;
+      CHECK(H_camera_target.isContinuous());
+      camera_target_list.push_back(H_camera_target.clone());
+    }
+
+    // Set H_field_camera
+    {
+      CHECK_EQ(cv::Size(3, 3), R_field_camera.size());
+      CHECK_EQ(cv::Size(1, 3), T_field_camera.size());
+      cv::Mat H_field_camera = cv::Mat::zeros(4, 4, CV_32F);
+      R_field_camera.copyTo(H_field_camera(cv::Range(0, 3), cv::Range(0, 3)));
+      T_field_camera.copyTo(H_field_camera(cv::Range(0, 3), cv::Range(3, 4)));
+      H_field_camera.at<float>(3, 3) = 1;
+      CHECK(H_field_camera.isContinuous());
+      field_camera_list.push_back(H_field_camera.clone());
+    }
+  }
+  // Now, send our two messages-- one large, with details for remote
+  // 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,
+                       target_point_vector, target_radius_vector,
+                       &result_sender_, false);
 }
 
 void CameraReader::ReadImage() {
@@ -310,13 +554,21 @@
                            const cv::Mat &descriptors) {
   const int number_features = keypoints.size();
   CHECK_EQ(descriptors.rows, number_features);
+  CHECK_EQ(descriptors.cols, 128);
   std::vector<flatbuffers::Offset<sift::Feature>> features_vector(
       number_features);
   for (int i = 0; i < number_features; ++i) {
-    const auto submat = descriptors(cv::Range(i, i + 1), cv::Range(0, 128));
+    const auto submat =
+        descriptors(cv::Range(i, i + 1), cv::Range(0, descriptors.cols));
     CHECK(submat.isContinuous());
-    const auto descriptor_offset =
-        fbb->CreateVector(reinterpret_cast<float *>(submat.data), 128);
+    flatbuffers::Offset<flatbuffers::Vector<uint8_t>> descriptor_offset;
+    {
+      uint8_t *data;
+      descriptor_offset = fbb->CreateUninitializedVector(128, &data);
+      submat.convertTo(
+          cv::Mat(1, descriptors.cols, CV_8U, static_cast<void *>(data)),
+          CV_8U);
+    }
     sift::Feature::Builder feature_builder(*fbb);
     feature_builder.add_descriptor(descriptor_offset);
     feature_builder.add_x(keypoints[i].pt.x);
@@ -334,9 +586,9 @@
 
 void CameraReaderMain() {
   aos::FlatbufferDetachedBuffer<aos::Configuration> config =
-      aos::configuration::ReadConfig("config.json");
+      aos::configuration::ReadConfig(FLAGS_config);
 
-  const auto training_data_bfbs = DemoSiftData();
+  const auto training_data_bfbs = SiftTrainingData();
   const sift::TrainingData *const training_data =
       flatbuffers::GetRoot<sift::TrainingData>(training_data_bfbs.data());
   {
@@ -354,8 +606,18 @@
   cv::FlannBasedMatcher matcher(index_params, search_params);
 
   aos::ShmEventLoop event_loop(&config.message());
+
+  // First, log the data for future reference.
+  {
+    aos::Sender<sift::TrainingData> training_data_sender =
+        event_loop.MakeSender<sift::TrainingData>("/camera");
+    training_data_sender.Send(
+        aos::FlatbufferString<sift::TrainingData>(training_data_bfbs));
+  }
+
   V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
-  CameraReader camera_reader(&event_loop, training_data, &v4l2_reader, &matcher);
+  CameraReader camera_reader(&event_loop, training_data, &v4l2_reader,
+                             &matcher);
 
   event_loop.Run();
 }
@@ -364,7 +626,6 @@
 }  // namespace vision
 }  // namespace frc971
 
-
 int main(int argc, char **argv) {
   aos::InitGoogle(&argc, &argv);
   frc971::vision::CameraReaderMain();
diff --git a/y2020/vision/rootfs/change_hostname.sh b/y2020/vision/rootfs/change_hostname.sh
index 1842504..712aba9 100755
--- a/y2020/vision/rootfs/change_hostname.sh
+++ b/y2020/vision/rootfs/change_hostname.sh
@@ -22,7 +22,7 @@
 
 echo "${HOSTNAME}" > /etc/hostname
 
-if grep /etc/hosts '^10\.[0-9]*\.[0-9]*\.[0-9]*\s*pi-[0-9]*-[0-9] pi[0-9]$' /etc/hosts >/dev/null ;
+if grep '^10\.[0-9]*\.[0-9]*\.[0-9]*\s*pi-[0-9]*-[0-9] pi[0-9]$' /etc/hosts >/dev/null ;
 then
   sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi[0-9]\)$/${IP_BASE}\1${TEAM_NUMBER}\2/" /etc/hosts
 else
diff --git a/y2020/vision/rootfs/frc971.service b/y2020/vision/rootfs/frc971.service
new file mode 100644
index 0000000..bf42652
--- /dev/null
+++ b/y2020/vision/rootfs/frc971.service
@@ -0,0 +1,9 @@
+[Unit]
+Description=Start up 971 robot code
+
+[Service]
+Type=oneshot
+ExecStart=su pi -c /home/pi/robot_code/starter.sh
+
+[Install]
+WantedBy=multi-user.target
diff --git a/y2020/vision/rootfs/modify_rootfs.sh b/y2020/vision/rootfs/modify_rootfs.sh
index c472caa..219590f 100755
--- a/y2020/vision/rootfs/modify_rootfs.sh
+++ b/y2020/vision/rootfs/modify_rootfs.sh
@@ -3,6 +3,7 @@
 set -xe
 
 IMAGE="2020-02-13-raspbian-buster-lite.img"
+BOOT_PARTITION="2020-02-13-raspbian-buster-lite.img.boot_partition"
 PARTITION="2020-02-13-raspbian-buster-lite.img.partition"
 HOSTNAME="pi-8971-1"
 
@@ -14,13 +15,60 @@
   USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" sudo -h 127.0.0.1 -u pi "$@"
 }
 
-OFFSET="$(fdisk -lu "${IMAGE}" | grep "${IMAGE}2" | awk '{print 512*$2}')"
+
 mkdir -p "${PARTITION}"
+mkdir -p "${BOOT_PARTITION}"
+
+if mount | grep "${BOOT_PARTITION}" >/dev/null ;
+then
+  echo "Already mounted"
+else
+  OFFSET="$(fdisk -lu "${IMAGE}" | grep "${IMAGE}1" | awk '{print 512*$2}')"
+  sudo mount -o loop,offset=${OFFSET} "${IMAGE}" "${BOOT_PARTITION}"
+fi
+
+# Enable the camera on boot.
+if ! grep "start_x=1" "${BOOT_PARTITION}/config.txt"; then
+  echo "start_x=1" | sudo tee -a "${BOOT_PARTITION}/config.txt"
+fi
+if ! grep "gpu_mem=128" "${BOOT_PARTITION}/config.txt"; then
+  echo "gpu_mem=128" | sudo tee -a "${BOOT_PARTITION}/config.txt"
+fi
+
+sudo umount "${BOOT_PARTITION}"
+rmdir "${BOOT_PARTITION}"
 
 if mount | grep "${PARTITION}" >/dev/null ;
 then
   echo "Already mounted"
 else
+  OFFSET="$(fdisk -lu "${IMAGE}" | grep "${IMAGE}2" | awk '{print 512*$2}')"
+
+  if [[ "$(stat -c %s "${IMAGE}")" == 1849688064 ]]; then
+    echo "Growing image"
+    dd if=/dev/zero bs=1M count=1024 >> "${IMAGE}"
+    START="$(fdisk -lu "${IMAGE}" | grep "${IMAGE}2" | awk '{print $2}')"
+
+    sed -e 's/\s*\([\+0-9a-zA-Z]*\).*/\1/' << EOF | fdisk "${IMAGE}"
+  d # remove old partition
+  2
+  n # new partition
+  p # primary partition
+  2 # partion number 2
+  532480 # start where the old one starts
+    # To the end
+  p # print the in-memory partition table
+  w # Flush
+  q # and we're done
+EOF
+
+    sudo losetup -o "${OFFSET}" -f "${IMAGE}"
+    LOOPBACK="$(sudo losetup --list | grep "${IMAGE}" | awk '{print $1}')"
+    sudo e2fsck -f "${LOOPBACK}"
+    sudo resize2fs "${LOOPBACK}"
+    sudo losetup -d "${LOOPBACK}"
+  fi
+
   echo "Mounting"
   sudo mount -o loop,offset=${OFFSET} "${IMAGE}" "${PARTITION}"
 fi
@@ -28,6 +76,7 @@
 sudo cp target_configure.sh "${PARTITION}/tmp/"
 sudo cp dhcpcd.conf "${PARTITION}/tmp/dhcpcd.conf"
 sudo cp change_hostname.sh "${PARTITION}/tmp/change_hostname.sh"
+sudo cp frc971.service "${PARTITION}/etc/systemd/system/frc971.service"
 
 target /bin/mkdir -p /home/pi/.ssh/
 cat ~/.ssh/id_rsa.pub | target tee /home/pi/.ssh/authorized_keys
diff --git a/y2020/vision/rootfs/target_configure.sh b/y2020/vision/rootfs/target_configure.sh
index 2a072e5..193f8b5 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/y2020/vision/rootfs/target_configure.sh
@@ -13,9 +13,44 @@
 
 chown -R pi.pi /home/pi/.ssh
 
+apt-get update
+
 apt-get install -y vim-nox \
   git \
-  cpufrequtils
+  python3-pip \
+  cpufrequtils \
+  libopencv-calib3d3.2 \
+  libopencv-contrib3.2 \
+  libopencv-core3.2 \
+  libopencv-features2d3.2 \
+  libopencv-flann3.2 \
+  libopencv-highgui3.2 \
+  libopencv-imgcodecs3.2 \
+  libopencv-imgproc3.2 \
+  libopencv-ml3.2 \
+  libopencv-objdetect3.2 \
+  libopencv-photo3.2 \
+  libopencv-shape3.2 \
+  libopencv-stitching3.2 \
+  libopencv-superres3.2 \
+  libopencv-video3.2 \
+  libopencv-videoio3.2 \
+  libopencv-videostab3.2 \
+  libopencv-viz3.2 \
+  python3-opencv \
+  gstreamer1.0-plugins-bad \
+  gstreamer1.0-plugins-base \
+  gstreamer1.0-plugins-good \
+  gstreamer1.0-plugins-ugly \
+  gstreamer1.0-x \
+  gstreamer1.0-nice \
+  gstreamer1.0-gl \
+  libgstreamer-plugins-bad1.0-0 \
+  libgstreamer-plugins-base1.0-0 \
+  libgstreamer1.0-0 \
+  libgstreamer-gl1.0-0 \
+  libnice10 \
+  libnice-dev
 
 echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
 
@@ -46,6 +81,7 @@
 rm -f /etc/profile.d/wifi-check.sh
 
 systemctl enable ssh.service
+systemctl enable frc971.service
 
 # Default us to pi-8971-1
 /root/bin/change_hostname.sh pi-8971-1
diff --git a/y2020/vision/sift/BUILD b/y2020/vision/sift/BUILD
index 02507a9..a7827b6 100644
--- a/y2020/vision/sift/BUILD
+++ b/y2020/vision/sift/BUILD
@@ -28,6 +28,7 @@
         # TODO(Brian): Replace this with something more fine-grained from the
         # configuration fragment or something.
         "//tools/cpp:toolchain",
+        "@amd64_debian_sysroot//:sysroot_files",
     ],
     default_python_version = "PY3",
     main = "fast_gaussian_runner.py",
@@ -195,10 +196,12 @@
     ],
     namespace = "frc971.vision.sift",
     tables = [
+        "KeypointFieldLocation",
         "Feature",
         "Match",
         "ImageMatch",
         "TransformationMatrix",
+        "CameraCalibration",
         "CameraPose",
         "ImageMatchResult",
         "TrainingImage",
diff --git a/y2020/vision/sift/demo_sift_training.py b/y2020/vision/sift/demo_sift_training.py
index a6650fd..c78a44a 100644
--- a/y2020/vision/sift/demo_sift_training.py
+++ b/y2020/vision/sift/demo_sift_training.py
@@ -26,7 +26,8 @@
   for keypoint, descriptor in zip(keypoints, descriptors):
     Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
     for n in reversed(descriptor):
-      fbb.PrependFloat32(n)
+      assert n == round(n)
+      fbb.PrependUint8(int(round(n)))
     descriptor_vector = fbb.EndVector(len(descriptor))
 
     Feature.FeatureStart(fbb)
@@ -49,14 +50,14 @@
   TrainingImage.TrainingImageStart(fbb)
   TrainingImage.TrainingImageAddFeatures(fbb, features_vector_table)
   # TODO(Brian): Fill out the transformation matrices.
-  training_image = TrainingImage.TrainingImageEnd(fbb)
+  training_image_offset = TrainingImage.TrainingImageEnd(fbb)
 
   TrainingData.TrainingDataStartImagesVector(fbb, 1)
-  fbb.PrependUOffsetTRelative(training_image)
-  images = fbb.EndVector(1)
+  fbb.PrependUOffsetTRelative(training_image_offset)
+  images_offset = fbb.EndVector(1)
 
   TrainingData.TrainingDataStart(fbb)
-  TrainingData.TrainingDataAddImages(fbb, images)
+  TrainingData.TrainingDataAddImages(fbb, images_offset)
   fbb.Finish(TrainingData.TrainingDataEnd(fbb))
 
   bfbs = fbb.Output()
diff --git a/y2020/vision/sift/fast_gaussian.bzl b/y2020/vision/sift/fast_gaussian.bzl
index a1c3173..0905423 100644
--- a/y2020/vision/sift/fast_gaussian.bzl
+++ b/y2020/vision/sift/fast_gaussian.bzl
@@ -1,55 +1,55 @@
 def fast_gaussian(sigmas, sizes):
-  files = []
-  for _, sigma_name, _ in sigmas:
+    files = []
+    for _, sigma_name, _ in sigmas:
+        for cols, rows in sizes:
+            files.append("fast_gaussian_%dx%d_%s" % (cols, rows, sigma_name))
+    for _, sigma_name, _ in sigmas:
+        for cols, rows in sizes:
+            files.append("fast_gaussian_subtract_%dx%d_%s" % (cols, rows, sigma_name))
     for cols, rows in sizes:
-      files.append("fast_gaussian_%dx%d_%s" % (cols, rows, sigma_name))
-  for _, sigma_name, _ in sigmas:
-    for cols, rows in sizes:
-      files.append("fast_gaussian_subtract_%dx%d_%s" % (cols, rows, sigma_name))
-  for cols, rows in sizes:
-    files.append('fast_subtract_%dx%d' % (cols, rows))
+        files.append("fast_subtract_%dx%d" % (cols, rows))
 
-  params = struct(
-    sigmas = sigmas,
-    sizes = sizes,
-  )
+    params = struct(
+        sigmas = sigmas,
+        sizes = sizes,
+    )
 
-  headers = [f + '.h' for f in files] + [
-    'fast_gaussian_all.h',
-  ]
-  objects = [f + '.o' for f in files] + [
-    'fast_gaussian_runtime.o',
-  ]
-  htmls = [f + '.html' for f in files]
+    headers = [f + ".h" for f in files] + [
+        "fast_gaussian_all.h",
+    ]
+    objects = [f + ".o" for f in files] + [
+        "fast_gaussian_runtime.o",
+    ]
+    htmls = [f + ".html" for f in files]
 
-  native.genrule(
-    name = "generate_fast_gaussian",
-    tools = [
-        ":fast_gaussian_runner",
-    ],
-    cmd = ' '.join([
-      '$(location fast_gaussian_runner)',
-      "'" + params.to_json() + "'",
-      # TODO(Brian): This should be RULEDIR once we have support for that.
-      '$(@D)',
-      '$(TARGET_CPU)',
-    ]),
-    outs = headers + objects + htmls,
-    restricted_to = [
-      "//tools:k8",
-      "//tools:armhf-debian",
-    ],
-  )
+    native.genrule(
+        name = "generate_fast_gaussian",
+        tools = [
+            ":fast_gaussian_runner",
+        ],
+        cmd = " ".join([
+            "$(location fast_gaussian_runner)",
+            "'" + params.to_json() + "'",
+            # TODO(Brian): This should be RULEDIR once we have support for that.
+            "$(@D)",
+            "$(TARGET_CPU)",
+        ]),
+        outs = headers + objects + htmls,
+        restricted_to = [
+            "//tools:k8",
+            "//tools:armhf-debian",
+        ],
+    )
 
-  native.cc_library(
-    name = 'fast_gaussian_all',
-    hdrs = ['fast_gaussian_all.h'],
-    srcs = headers + objects,
-    deps = [
-      '//third_party:halide_runtime',
-    ],
-    restricted_to = [
-      "//tools:k8",
-      "//tools:armhf-debian",
-    ],
-  )
+    native.cc_library(
+        name = "fast_gaussian_all",
+        hdrs = ["fast_gaussian_all.h"],
+        srcs = headers + objects,
+        deps = [
+            "//third_party:halide_runtime",
+        ],
+        restricted_to = [
+            "//tools:k8",
+            "//tools:armhf-debian",
+        ],
+    )
diff --git a/y2020/vision/sift/fast_gaussian_runner.py b/y2020/vision/sift/fast_gaussian_runner.py
index 9699fef..6faa9fc 100755
--- a/y2020/vision/sift/fast_gaussian_runner.py
+++ b/y2020/vision/sift/fast_gaussian_runner.py
@@ -22,12 +22,14 @@
 
   commands = []
 
+  amd64_debian_sysroot = r.Rlocation('amd64_debian_sysroot/usr/lib/x86_64-linux-gnu/libc.so.6').rsplit('/', 4)[0]
   env = os.environ.copy()
   env['LD_LIBRARY_PATH'] = ':'.join([
-      'debian_amd64_sysroot/lib/x86_64-linux-gnu',
-      'debian_amd64_sysroot/lib',
-      'debian_amd64_sysroot/usr/lib/x86_64-linux-gnu',
-      'debian_amd64_sysroot/usr/lib',
+      # TODO(brian): Figure this out again.  It is a bit aggressive.
+      #amd64_debian_sysroot + '/lib/x86_64-linux-gnu',
+      #amd64_debian_sysroot + '/lib',
+      #amd64_debian_sysroot + '/usr/lib/x86_64-linux-gnu',
+      #amd64_debian_sysroot + '/usr/lib',
   ])
 
   all_header = [
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 97c2b0a..24fd64d 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -9,7 +9,8 @@
 
 // Represents a single feature extracted from an image.
 table Feature {
-  // Contains the descriptor data.
+  // Contains the descriptor data. OpenCV likes to represent them as floats, but
+  // they're really ubytes.
   //
   // TODO(Brian): These are scaled to be convertible to chars. Should we do
   // that to minimize storage space? Or maybe int16?
@@ -17,7 +18,7 @@
   // The size of this depends on the parameters. It is width*width*hist_bins.
   // Currently we have width=4 and hist_bins=8, which results in a size of
   // 4*4*8=128.
-  descriptor:[float];
+  descriptor:[ubyte];
 
   // Location of the keypoint.
   x:float;
@@ -60,8 +61,8 @@
 }
 
 table TransformationMatrix {
-  // The matrix data. This is a row-major 4x4 matrix.
-  // In other words, the bottom row is (0, 0, 0, 1).
+  // The matrix data for a row-major 4x4 homogeneous transformation matrix.
+  // This implies the bottom row is (0, 0, 0, 1).
   data:[float];
 }
 
@@ -97,21 +98,29 @@
   //   rotation around the Z axis by the turret angle
   //   turret_extrinsics
   turret_extrinsics:TransformationMatrix;
+
+  // This is the standard OpenCV 5 parameter distortion coefficients
+  dist_coeffs:[float];
 }
 
 // Contains the information the EKF wants from an image matched against a single
 // training image.
 //
-// This is represented as a transformation to a target in field coordinates.
+// This is represented as a transformation from the camera to the target
+// (camera_to_target) and a transformatoin from the field to the target
+// (field_to_target).
+//
+// We also send the map from the field to the camera, which can be computed
+// with the first two, to make it easier to display.
 table CameraPose {
-  // Transformation matrix from the target to the camera's origin.
+  // Transformation matrix from the camera to the target.
   // (0, 0, 0) is the aperture of the camera (we pretend it's an ideal pinhole
   // camera). Positive Z is out of the camera. Positive X and Y are right
   // handed, but which way they face depends on the camera extrinsics.
   camera_to_target:TransformationMatrix;
 
   // Field coordinates of the target, represented as a transformation matrix
-  // from the target to the field.
+  // from the field to the target.
   // (0, 0, 0) is the center of the field, on the level of most of the field
   // (not the region under the truss). Positive X is towards the red alliance's
   // PLAYER STATION. Positive Z is up. The coordinate system is right-handed.
@@ -122,12 +131,22 @@
   // The value here will be selected from a small, static set of targets we
   // train images on.
   field_to_target:TransformationMatrix;
+
+  // 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 {
   // The matches from this image to each of the training images which matched.
   // Each member is against the same captured image.
   image_matches:[ImageMatch];
+
   // The transformations for this image for each of the training images which
   // matched.
   // TODO(Brian): Include some kind of covariance information for these.
diff --git a/y2020/vision/sift/sift_training.fbs b/y2020/vision/sift/sift_training.fbs
index 7391e76..12ad9b4 100644
--- a/y2020/vision/sift/sift_training.fbs
+++ b/y2020/vision/sift/sift_training.fbs
@@ -10,6 +10,12 @@
   // from the target to the field. See CameraPose in :sift_fbs for details of
   // the conventions of this.
   field_to_target:TransformationMatrix;
+
+  // 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/BUILD b/y2020/vision/tools/python_code/BUILD
index 670b664..1892a88 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -2,27 +2,26 @@
 
 py_binary(
     name = "load_sift_training",
-    data = [
-        ":test_images/train_power_port_red.png",
-        ":test_images/train_power_port_red_webcam.png",
-        ":test_images/train_power_port_blue.png",
-        ":test_images/train_loading_bay_red.png",
-        ":test_images/train_loading_bay_blue.png",
-    ],
-    srcs = ["load_sift_training.py",
+    srcs = [
         "camera_definition.py",
         "define_training_data.py",
+        "load_sift_training.py",
         "target_definition.py",
         "train_and_match.py",
     ],
-    args = ["sift_training_data.h",
+    args = [
+        "sift_training_data.h",
     ],
+    data = glob(["calib_files/*.txt"]) + glob([
+        "test_images/*.png",
+    ]),
     default_python_version = "PY3",
     srcs_version = "PY2AND3",
     deps = [
+        "//external:python-glog",
         "//y2020/vision/sift:sift_fbs_python",
-        "@opencv_contrib_nonfree_amd64//:python_opencv",
         "@bazel_tools//tools/python/runfiles",
+        "@opencv_contrib_nonfree_amd64//:python_opencv",
     ],
 )
 
@@ -41,9 +40,77 @@
 )
 
 cc_library(
-    name = "sift_training",
+    name = "sift_training_data",
     hdrs = [
         "sift_training_data.h",
     ],
     visibility = ["//visibility:public"],
 )
+
+py_binary(
+    name = "load_sift_training_test",
+    srcs = [
+        "camera_definition_test.py",
+        "define_training_data.py",
+        "load_sift_training.py",
+        "target_definition_test.py",
+        "train_and_match.py",
+    ],
+    args = [
+        "sift_training_data_test.h",
+        "test",
+    ],
+    data = glob(["calib_files/*.txt"]) + glob([
+        "test_images/*.png",
+    ]),
+    default_python_version = "PY3",
+    main = "load_sift_training.py",
+    srcs_version = "PY2AND3",
+    deps = [
+        "//external:python-glog",
+        "//y2020/vision/sift:sift_fbs_python",
+        "@bazel_tools//tools/python/runfiles",
+        "@opencv_contrib_nonfree_amd64//:python_opencv",
+    ],
+)
+
+genrule(
+    name = "run_load_sift_training_test",
+    outs = [
+        "sift_training_data_test.h",
+    ],
+    cmd = " ".join([
+        "$(location :load_sift_training_test)",
+        "$(location sift_training_data_test.h) test",
+    ]),
+    tools = [
+        ":load_sift_training_test",
+    ],
+)
+
+cc_library(
+    name = "sift_training_data_test",
+    hdrs = [
+        "sift_training_data_test.h",
+    ],
+    visibility = ["//visibility:public"],
+)
+
+cc_test(
+    name = "camera_param_test",
+    srcs = [
+        "camera_param_test.cc",
+    ],
+    restricted_to = [
+        "//tools:k8",
+        "//tools:armhf-debian",
+    ],
+    deps = [
+        ":sift_training_data_test",
+        "//aos/testing:googletest",
+        "//third_party:opencv",
+        "//y2020/vision:vision_fbs",
+        "//y2020/vision/sift:sift_fbs",
+        "//y2020/vision/sift:sift_training_fbs",
+    ],
+)
diff --git a/y2020/vision/tools/python_code/calib_files/cam-calib-int_pi-7971-3_Feb-13-2020-00-00-00.txt b/y2020/vision/tools/python_code/calib_files/cam-calib-int_pi-7971-3_Feb-13-2020-00-00-00.txt
new file mode 100644
index 0000000..8c1efbb
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/cam-calib-int_pi-7971-3_Feb-13-2020-00-00-00.txt
@@ -0,0 +1 @@
+{"hostname": "pi-7971-3", "node_name": "pi3", "team_number": 7971, "timestamp": "Feb-13-2020-00-00-00", "camera_matrix": [[388.79947319784713, 0.0, 345.0976031055917], [0.0, 388.539148344188, 265.2780372766764], [0.0, 0.0, 1.0]], "dist_coeffs": [[0.13939139612079282, -0.24345067782097646, -0.0004228219772016648, -0.0004552350162154737, 0.08966339831250879]]}
diff --git a/y2020/vision/tools/python_code/calibrate_intrinsics.py b/y2020/vision/tools/python_code/calibrate_intrinsics.py
new file mode 100644
index 0000000..828a9a9
--- /dev/null
+++ b/y2020/vision/tools/python_code/calibrate_intrinsics.py
@@ -0,0 +1,133 @@
+import cv2
+import cv2.aruco
+import datetime
+import glog
+import json
+from json import JSONEncoder
+import numpy as np
+import os
+import time
+
+
+# From: https://pynative.com/python-serialize-numpy-ndarray-into-json/
+class NumpyArrayEncoder(json.JSONEncoder):
+    def default(self, obj):
+        if isinstance(obj, np.integer):
+            return int(obj)
+        elif isinstance(obj, np.floating):
+            return float(obj)
+        elif isinstance(obj, np.ndarray):
+            return obj.tolist()
+        else:
+            return super(NumpyArrayEncoder, self).default(obj)
+
+
+def get_robot_info(hostname):
+    hostname_split = hostname[1].split("-")
+    if hostname_split[0] != "pi":
+        print("ERROR: expected hostname to start with pi!")
+        quit()
+
+    team_number = int(hostname_split[1])
+    node_name = hostname_split[0] + hostname_split[2]
+    return node_name, team_number
+
+
+USE_LARGE_BOARD = False
+
+if USE_LARGE_BOARD:
+    dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_100)
+    # Need to measure what the second size parameter is (markerLength)
+    board = cv2.aruco.CharucoBoard_create(12, 9, .06, .045, dictionary)
+    img = board.draw((200 * 12, 200 * 9))
+else:
+    dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+    board = cv2.aruco.CharucoBoard_create(11, 8, .015, .011, dictionary)
+    img = board.draw((200 * 11, 200 * 8))
+
+#Dump the calibration board to a file
+cv2.imwrite('charuco.png', img)
+
+#Start capturing images for calibration
+CAMERA_INDEX = 0  # Capture from /dev/videoX, where X=CAMERA_INDEX
+cap = cv2.VideoCapture(CAMERA_INDEX)
+
+allCorners = []
+allIds = []
+capture_count = 0
+MIN_IMAGES = 50
+
+while (capture_count < MIN_IMAGES):
+
+    ret, frame = cap.read()
+    assert ret, "Unable to get image from the camera at /dev/video%d" % CAMERA_INDEX
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    res = cv2.aruco.detectMarkers(gray, dictionary)
+    aruco_detect_image = frame.copy()
+
+    if len(res[0]) > 0 and len(res[1]) > 0:
+        cv2.aruco.drawDetectedMarkers(aruco_detect_image, res[0], res[1])
+
+    # Display every image to let user trigger capture
+    cv2.imshow('frame', aruco_detect_image)
+    keystroke = cv2.waitKey(1)
+
+    if keystroke & 0xFF == ord('q'):
+        break
+    elif keystroke & 0xFF == ord('c'):
+        glog.info("Asked to capture image")
+        if len(res[0]) == 0 or len(res[1]) == 0:
+            # Can't use this image
+            continue
+
+        res2 = cv2.aruco.interpolateCornersCharuco(res[0], res[1], gray, board)
+        if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3:
+            capture_count += 1
+            charuco_detect_image = frame.copy()
+            allCorners.append(res2[1])
+            allIds.append(res2[2])
+            glog.info("Capturing image #%d" % capture_count)
+            cv2.aruco.drawDetectedCornersCharuco(charuco_detect_image, res2[1],
+                                                 res2[2])
+
+            cv2.imshow('frame', charuco_detect_image)
+            cv2.waitKey(1000)
+            # TODO<Jim>: Should log image to disk
+
+#Calibration fails for lots of reasons. Release the video if we do
+try:
+    imsize = gray.shape
+    cal = cv2.aruco.calibrateCameraCharuco(allCorners, allIds, board, imsize,
+                                           None, None)
+    glog.info("Calibration is:\n", cal)
+    glog.info("Reproduction error:", cal[0])
+    if (cal[0] > 1.0):
+        glog.error("REPRODUCTION ERROR NOT GOOD")
+    glog.info("Calibration matrix:\n", cal[1])
+    glog.info("Distortion Coefficients:\n", cal[2])
+except:
+    glog.error("Calibration failed")
+    cap.release()
+    quit()
+
+hostname = os.uname()[1]
+date_str = datetime.datetime.today().strftime("%Y-%m-%d-%H-%M-%S")
+node_name, team_number = get_robot_info(hostname)
+numpyData = {
+    "hostname": hostname,
+    "node_name": node_name,
+    "team_number": team_number,
+    "timestamp": date_str,
+    "camera_matrix": cal[1],
+    "dist_coeffs": cal[2]
+}
+encodedNumpyData = json.dumps(
+    numpyData, cls=NumpyArrayEncoder)  # use dump() to write array into file
+
+# Write out the data
+calib_file = open("cam_calib_%s_%s.txt" % (hostname, date_str), "w")
+calib_file.write(encodedNumpyData)
+calib_file.close()
+
+cap.release()
+cv2.destroyAllWindows()
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 59f008f..4d8929a 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -1,31 +1,42 @@
-import argparse
-import cv2
+import copy
+import glog
 import json
 import math
 import numpy as np
-import time
+import os
+
+glog.setLevel("WARN")
+USE_BAZEL = True
+
+
+def bazel_name_fix(filename):
+    ret_name = filename
+    if USE_BAZEL:
+        ret_name = 'org_frc971/y2020/vision/tools/python_code/' + filename
+
+    return ret_name
 
 
 class CameraIntrinsics:
     def __init__(self):
         self.camera_matrix = []
-        self.distortion_coeffs = []
+        self.dist_coeffs = []
 
     pass
 
+
 class CameraExtrinsics:
     def __init__(self):
         self.R = []
         self.T = []
 
-    pass
 
 class CameraParameters:
     def __init__(self):
         self.camera_int = CameraIntrinsics()
         self.camera_ext = CameraExtrinsics()
-
-    pass
+        self.node_name = ""
+        self.team_number = -1
 
 
 ### CAMERA DEFINITIONS
@@ -43,15 +54,54 @@
 # Define a web_cam
 web_cam_int = CameraIntrinsics()
 web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
-web_cam_int.distortion_coeffs = np.zeros((5,1))
+web_cam_int.dist_coeffs = np.zeros((5, 1))
 
 web_cam_ext = CameraExtrinsics()
 # Camera rotation from robot x,y,z to opencv (z, -x, -y)
-web_cam_ext.R = np.array([[0., 0., 1.],
-                          [-1, 0,  0],
-                          [0, -1., 0]])
-web_cam_ext.T = np.array([[0., 0., 0.]]).T
+web_cam_ext.R = np.array([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]])
+web_cam_ext.T = np.array([0., 0., 0.])
 
 web_cam_params = CameraParameters()
 web_cam_params.camera_int = web_cam_int
 web_cam_params.camera_ext = web_cam_ext
+
+camera_list = []
+
+# TODO<Jim>: Should probably make this a dict to make replacing easier
+for team_number in (971, 7971, 8971, 9971):
+    for node_name in ("pi0", "pi1", "pi2", "pi3", "pi4", "pi5"):
+        camera_base = copy.deepcopy(web_cam_params)
+        camera_base.node_name = node_name
+        camera_base.team_number = team_number
+        camera_list.append(camera_base)
+
+dir_name = ('calib_files')
+
+if USE_BAZEL:
+    from bazel_tools.tools.python.runfiles import runfiles
+    r = runfiles.Create()
+    dir_name = r.Rlocation(bazel_name_fix('calib_files'))
+
+for filename in os.listdir(dir_name):
+    if "cam-calib-int" in filename and filename.endswith(".txt"):
+        # Extract intrinsics from file
+        fn_split = filename.split("_")
+        hostname_split = fn_split[1].split("-")
+        if hostname_split[0] == "pi":
+            team_number = int(hostname_split[1])
+            node_name = hostname_split[0] + hostname_split[2]
+
+        calib_file = open(dir_name + "/" + filename, 'r')
+        calib_dict = json.loads(calib_file.read())
+        hostname = np.asarray(calib_dict["hostname"])
+        camera_matrix = np.asarray(calib_dict["camera_matrix"])
+        dist_coeffs = np.asarray(calib_dict["dist_coeffs"])
+
+        # Look for match, and replace camera_intrinsics
+        for camera_calib in camera_list:
+            if camera_calib.node_name == node_name and camera_calib.team_number == team_number:
+                glog.info("Found calib for %s, team #%d" % (node_name,
+                                                            team_number))
+                camera_calib.camera_int.camera_matrix = copy.copy(
+                    camera_matrix)
+                camera_calib.camera_int.dist_coeffs = copy.copy(dist_coeffs)
diff --git a/y2020/vision/tools/python_code/camera_definition_test.py b/y2020/vision/tools/python_code/camera_definition_test.py
new file mode 100644
index 0000000..732dbb8
--- /dev/null
+++ b/y2020/vision/tools/python_code/camera_definition_test.py
@@ -0,0 +1,61 @@
+import copy
+import math
+import numpy as np
+
+
+class CameraIntrinsics:
+    def __init__(self):
+        self.camera_matrix = []
+        self.dist_coeffs = []
+
+
+class CameraExtrinsics:
+    def __init__(self):
+        self.R = []
+        self.T = []
+
+
+class CameraParameters:
+    def __init__(self):
+        self.camera_int = CameraIntrinsics()
+        self.camera_ext = CameraExtrinsics()
+        self.node_name = ""
+        self.team_number = -1
+
+
+### CAMERA DEFINITIONS
+
+# Robot camera has:
+# FOV_H = 93.*math.pi()/180.
+# FOV_V = 70.*math.pi()/180.
+
+# Create fake camera (based on USB webcam params)
+fx = 810.
+fy = 810.
+cx = 320.
+cy = 240.
+
+# Define a web_cam
+web_cam_int = CameraIntrinsics()
+web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
+web_cam_int.dist_coeffs = np.zeros((5, 1))
+
+web_cam_ext = CameraExtrinsics()
+# Camera rotation from robot x,y,z to opencv (z, -x, -y)
+web_cam_ext.R = np.array([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]])
+web_cam_ext.T = np.array([0., 0., 0.])
+
+web_cam_params = CameraParameters()
+web_cam_params.camera_int = web_cam_int
+web_cam_params.camera_ext = web_cam_ext
+
+camera_list = []
+
+for team_number in (971, 8971, 9971):
+    for (i, node_name) in enumerate(("pi-1", "pi-2", "pi-3", "pi-4", "pi-5")):
+        camera_base = copy.deepcopy(web_cam_params)
+        camera_base.node_name = node_name
+        camera_base.team_number = team_number
+        camera_base.camera_ext.T = np.asarray(
+            np.float32([i + 1, i + 1, i + 1]))
+        camera_list.append(camera_base)
diff --git a/y2020/vision/tools/python_code/camera_param_test.cc b/y2020/vision/tools/python_code/camera_param_test.cc
new file mode 100644
index 0000000..483fe75
--- /dev/null
+++ b/y2020/vision/tools/python_code/camera_param_test.cc
@@ -0,0 +1,279 @@
+#include <unistd.h>
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#if 1
+#include "y2020/vision/tools/python_code/sift_training_data_test.h"
+#else
+// Using this include (and changing BUILD file) allows to test on real data
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#endif
+
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/vision_generated.h"
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+class Feature {
+ public:
+  std::vector<float> descriptor_;
+  /*	float x_;
+  float y_;
+  float size_;
+  float angle_;
+  float response_;
+  int octave_;
+  cv::Point3f keypoint_field_location_;*/
+};
+
+class CameraCalibration {
+ public:
+  cv::Mat intrinsics_;
+  cv::Mat fixed_extrinsics_;
+};
+
+class TrainingImage {
+ public:
+  cv::Mat features_;
+  float target_point_x_;
+  float target_point_y_;
+  cv::Mat field_to_target_;
+};
+
+class TrainingData {
+ public:
+  std::vector<TrainingImage> images_;
+  CameraCalibration camera_calibration_;
+};
+
+class CameraParamTest {
+ public:
+  CameraParamTest() {
+    const auto training_data_bfbs = SiftTrainingData();
+    sift_training_data_ =
+        flatbuffers::GetRoot<sift::TrainingData>(training_data_bfbs.data());
+    {
+      flatbuffers::Verifier verifier(
+          reinterpret_cast<const uint8_t *>(training_data_bfbs.data()),
+          training_data_bfbs.size());
+
+      CHECK(sift_training_data_->Verify(verifier));
+    }
+
+    CopyTrainingFeatures();
+    sift_camera_calibration_ = CameraParamTest::FindCameraCalibration();
+    camera_intrinsic_matrix_ = CameraIntrinsicMatrix();
+    camera_dist_coeffs_ = CameraDistCoeffs();
+    camera_extrinsics_ = CameraExtrinsics();
+  }
+
+  // Copies the information from sift_training_data_ into matcher_.
+  void CopyTrainingFeatures();
+
+  const sift::CameraCalibration *FindCameraCalibration() const;
+
+  // Returns the 2D image location for the specified training feature.
+  cv::Point2f Training2dPoint(int training_image_index, int feature_index);
+  // Returns the 3D location for the specified training feature.
+  cv::Point3f Training3dPoint(int training_image_index, int feature_index);
+
+  const sift::TransformationMatrix *FieldToTarget(int training_image_index) {
+    return sift_training_data_->images()
+        ->Get(training_image_index)
+        ->field_to_target();
+  }
+
+  cv::Mat CameraIntrinsicMatrix() const {
+    const cv::Mat result(3, 3, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             sift_camera_calibration_->intrinsics()->data())));
+    CHECK_EQ(result.total(), sift_camera_calibration_->intrinsics()->size());
+    return result;
+  }
+
+  cv::Mat CameraDistCoeffs() const {
+    const cv::Mat result(5, 1, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             sift_camera_calibration_->dist_coeffs()->data())));
+    CHECK_EQ(result.total(), sift_camera_calibration_->dist_coeffs()->size());
+    return result;
+  }
+
+  cv::Mat CameraExtrinsics() const {
+    const cv::Mat result(
+        4, 4, CV_32F,
+        const_cast<void *>(static_cast<const void *>(
+            sift_camera_calibration_->fixed_extrinsics()->data()->data())));
+    return result;
+  }
+
+  int number_training_images() const {
+    return sift_training_data_->images()->size();
+  }
+
+  const std::string node_name_ = "pi-3";
+  const int team_number_ = 971;
+
+  // We'll just extract the one that matches our current module
+  const sift::CameraCalibration *sift_camera_calibration_;
+  cv::Mat camera_intrinsic_matrix_;
+  cv::Mat camera_dist_coeffs_;
+  cv::Mat camera_extrinsics_;
+
+  TrainingData training_data_;
+  const sift::TrainingData *sift_training_data_;
+  std::vector<sift::TransformationMatrix *> field_to_targets_;
+  std::vector<cv::Point2f> point_list_2d_;
+  std::vector<cv::Point3f> point_list_3d_;
+};
+
+// Copy in the keypoint descriptors, the 2d (image) location,
+// and the 3d (field) location
+void CameraParamTest::CopyTrainingFeatures() {
+  int train_image_index = 0;
+  for (const sift::TrainingImage *training_image :
+       *sift_training_data_->images()) {
+    TrainingImage tmp_training_image_data;
+    cv::Mat features(training_image->features()->size(), 128, CV_32F);
+    for (size_t i = 0; i < training_image->features()->size(); ++i) {
+      const sift::Feature *feature_table = training_image->features()->Get(i);
+      const flatbuffers::Vector<uint8_t> *const descriptor =
+          feature_table->descriptor();
+      CHECK_EQ(descriptor->size(), 128u) << ": Unsupported feature size";
+      const auto in_mat = cv::Mat(
+          1, descriptor->size(), CV_8U,
+          const_cast<void *>(static_cast<const void *>(descriptor->data())));
+      const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
+      in_mat.convertTo(out_mat, CV_32F);
+
+      cv::Point2f point_2d = Training2dPoint(train_image_index, i);
+      point_list_2d_.push_back(point_2d);
+      cv::Point3f point_3d = Training3dPoint(train_image_index, i);
+      point_list_3d_.push_back(point_3d);
+    }
+    const sift::TransformationMatrix *field_to_target_ =
+        FieldToTarget(train_image_index);
+    const cv::Mat field_to_target_mat(
+        4, 4, CV_32F,
+        const_cast<void *>(
+            static_cast<const void *>(field_to_target_->data()->data())));
+    tmp_training_image_data.features_ = features;
+    tmp_training_image_data.field_to_target_ = field_to_target_mat;
+    tmp_training_image_data.target_point_x_ =
+        sift_training_data_->images()->Get(train_image_index)->target_point_x();
+    tmp_training_image_data.target_point_y_ =
+        sift_training_data_->images()->Get(train_image_index)->target_point_y();
+
+    training_data_.images_.push_back(tmp_training_image_data);
+    train_image_index++;
+  }
+}
+
+const sift::CameraCalibration *CameraParamTest::FindCameraCalibration() const {
+  for (const sift::CameraCalibration *candidate :
+       *sift_training_data_->camera_calibrations()) {
+    if (candidate->node_name()->string_view() != node_name_) {
+      continue;
+    }
+    if (candidate->team_number() != team_number_) {
+      continue;
+    }
+    return candidate;
+  }
+  LOG(INFO) << ": Failed to find camera calibration for " << node_name_
+            << " on " << team_number_;
+  return NULL;
+}
+
+// Returns the 2D image location for the specified training feature.
+cv::Point2f CameraParamTest::Training2dPoint(int training_image_index,
+                                             int feature_index) {
+  const float x = sift_training_data_->images()
+                      ->Get(training_image_index)
+                      ->features()
+                      ->Get(feature_index)
+                      ->x();
+  const float y = sift_training_data_->images()
+                      ->Get(training_image_index)
+                      ->features()
+                      ->Get(feature_index)
+                      ->y();
+  return cv::Point2f(x, y);
+}
+
+// Returns the 3D location for the specified training feature.
+cv::Point3f CameraParamTest::Training3dPoint(int training_image_index,
+                                             int feature_index) {
+  const sift::KeypointFieldLocation *const location =
+      sift_training_data_->images()
+          ->Get(training_image_index)
+          ->features()
+          ->Get(feature_index)
+          ->field_location();
+  return cv::Point3f(location->x(), location->y(), location->z());
+}
+
+TEST(CameraParamTest, TargetDataTest) {
+  CameraParamTest camera_params;
+
+  ASSERT_EQ(camera_params.number_training_images(), 1);
+  ASSERT_EQ(camera_params.point_list_2d_.size(), 676);
+  ASSERT_EQ(camera_params.point_list_2d_[0].x, 0);
+  ASSERT_EQ(camera_params.point_list_2d_[0].y, 1);
+  ASSERT_EQ(camera_params.point_list_3d_.size(), 676);
+  ASSERT_EQ(camera_params.point_list_3d_[0].x, 0);
+  ASSERT_EQ(camera_params.point_list_3d_[1].y, 1);
+  ASSERT_EQ(camera_params.point_list_3d_[2].z, 2);
+
+  float ftt_mat[16] = {1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 2, 0, 0, 0, 1};
+  cv::Mat field_to_targets_0 = cv::Mat(4, 4, CV_32F, ftt_mat);
+  cv::Mat ftt_diff =
+      (camera_params.training_data_.images_[0].field_to_target_ !=
+       field_to_targets_0);
+  bool ftt_eq = (cv::countNonZero(ftt_diff) == 0);
+  ASSERT_TRUE(ftt_eq)
+      << "Mismatch on field_to_target: "
+      << camera_params.training_data_.images_[0].field_to_target_ << "\nvs.\n"
+      << field_to_targets_0;
+
+  ASSERT_EQ(camera_params.training_data_.images_[0].target_point_x_, 10.);
+  ASSERT_EQ(camera_params.training_data_.images_[0].target_point_y_, 20.);
+
+  float intrinsic_mat[9] = {810, 0, 320, 0, 810, 240, 0, 0, 1};
+  cv::Mat intrinsic = cv::Mat(3, 3, CV_32F, intrinsic_mat);
+  cv::Mat intrinsic_diff =
+      (intrinsic != camera_params.camera_intrinsic_matrix_);
+  bool intrinsic_eq = (cv::countNonZero(intrinsic_diff) == 0);
+  ASSERT_TRUE(intrinsic_eq)
+      << "Mismatch on camera intrinsic matrix: " << intrinsic << "\nvs.\n"
+      << camera_params.camera_intrinsic_matrix_;
+
+  float dist_coeffs_mat[5] = {0., 0., 0., 0., 0.};
+  cv::Mat dist_coeffs = cv::Mat(5, 1, CV_32F, dist_coeffs_mat);
+  cv::Mat dist_coeffs_diff = (dist_coeffs != camera_params.camera_dist_coeffs_);
+  bool dist_coeffs_eq = (cv::countNonZero(dist_coeffs_diff) == 0);
+  ASSERT_TRUE(dist_coeffs_eq)
+      << "Mismatch on camera distortion coefficients: " << dist_coeffs
+      << "\nvs.\n"
+      << camera_params.camera_dist_coeffs_;
+
+  float i_f = 3.0;
+  float extrinsic_mat[16] = {0, 0,  1, i_f, -1, 0, 0, i_f,
+                             0, -1, 0, i_f, 0,  0, 0, 1};
+  cv::Mat extrinsic = cv::Mat(4, 4, CV_32F, extrinsic_mat);
+  cv::Mat extrinsic_diff = (extrinsic != camera_params.camera_extrinsics_);
+  bool extrinsic_eq = (cv::countNonZero(extrinsic_diff) == 0);
+  ASSERT_TRUE(extrinsic_eq)
+      << "Mismatch of extrinsic: " << extrinsic << "\nvs.\n"
+      << camera_params.camera_extrinsics_;
+}
+
+}  // namespace
+}  // namespace vision
+}  // namespace frc971
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
index 22eb6ce..8116fb7 100644
--- a/y2020/vision/tools/python_code/define_training_data.py
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -1,5 +1,6 @@
 import argparse
 import cv2
+import glog
 import json
 import math
 import numpy as np
@@ -17,7 +18,7 @@
     global current_mouse
     current_mouse = (x, y)
     if event == cv2.EVENT_LBUTTONUP:
-        #print("Adding point at %d, %d" % (x,y))
+        glog.debug("Adding point at %d, %d" % (x, y))
         point_list.append([x, y])
     pass
 
@@ -142,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 = []
@@ -165,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)
@@ -174,51 +178,56 @@
     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):
+def visualize_reprojections(img, pts_2d, pts_3d, cam_mat, dist_coeffs):
     # Compute camera location
     # TODO: Warn on bad inliers
     # TODO: Change this to not have to recast to np
     pts_2d_np = np.asarray(np.float32(pts_2d)).reshape(-1, 1, 2)
     pts_3d_np = np.asarray(np.float32(pts_3d)).reshape(-1, 1, 3)
     retval, R, T, inliers = cv2.solvePnPRansac(pts_3d_np, pts_2d_np, cam_mat,
-                                               distortion_coeffs)
+                                               dist_coeffs)
     pts_3d_proj_2d, jac_2d = cv2.projectPoints(pts_3d_np, R, T, cam_mat,
-                                               distortion_coeffs)
+                                               dist_coeffs)
     if inliers is None:
-        print("WARNING: Didn't get any inliers when reprojecting polygons")
+        glog.warn("WARNING: Didn't get any inliers when reprojecting polygons")
         return img
     for i in range(len(pts_2d)):
         pt_2d = pts_2d_np[i][0]
@@ -239,17 +248,14 @@
     image = cv2.imread("test_images/train_power_port_red.png")
 
     polygon_list = define_polygon(image)
-    print(polygon_list)
+    glog.debug(polygon_list)
 
 
 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)
-    print(polygon_list)
-
+    glog.debug(polygon_list)
diff --git a/y2020/vision/tools/python_code/field_display.py b/y2020/vision/tools/python_code/field_display.py
index 93f645c..11d098d 100644
--- a/y2020/vision/tools/python_code/field_display.py
+++ b/y2020/vision/tools/python_code/field_display.py
@@ -17,17 +17,18 @@
 
 
 # Convert field coordinates (meters) to image coordinates (pixels).
-# Field origin is (x,y) at center of red alliance driver station wall,
-# with x pointing into the field.
+# Field origin is (x,y) at center of the field,
+# with x pointing towards the red alliance driver station
 # The default field image is 1598 x 821 pixels, so 1 cm per pixel on field
 # I.e., Field is 1598 x 821 pixels = 15.98 x 8.21 meters
-# Our origin in image coordinates is at (1598, 410) pixels, facing in the -x image direction
+# Our origin in image coordinates is at (799, 410) pixels, with +x direction
+# to the right
 
 
 def field_coord_to_image_coord(robot_position):
     # Scale by 100 pixel / cm
-    robot_pos_img_coord = np.array([[1598, 410]]).T + np.int32(
-        100.0 * np.array([[-robot_position[0][0], robot_position[1][0]]]).T)
+    robot_pos_img_coord = np.array([[799, 410]]).T + np.int32(
+        100.0 * np.array([[robot_position[0][0], -robot_position[1][0]]]).T)
     return robot_pos_img_coord
 
 
diff --git a/y2020/vision/tools/python_code/image_capture.py b/y2020/vision/tools/python_code/image_capture.py
new file mode 100644
index 0000000..d6ce6d7
--- /dev/null
+++ b/y2020/vision/tools/python_code/image_capture.py
@@ -0,0 +1,34 @@
+import cv2
+import datetime
+# Open the device at the ID X for /dev/videoX
+CAMERA_INDEX = 0
+cap = cv2.VideoCapture(CAMERA_INDEX)
+
+#Check whether user selected camera is opened successfully.
+if not (cap.isOpened()):
+    print("Could not open video device /dev/video%d" % CAMERA_INDEX)
+    quit()
+
+while True:
+    # Capture frame-by-frame
+    ret, frame = cap.read()
+
+    exp = cap.get(cv2.CAP_PROP_EXPOSURE)
+    #print("Exposure:", exp)
+    # Display the resulting frame
+    cv2.imshow('preview', frame)
+
+    #Waits for a user input to capture image or quit the application
+    keystroke = cv2.waitKey(1)
+
+    if keystroke & 0xFF == ord('q'):
+        break
+    elif keystroke & 0xFF == ord('c'):
+        image_name = datetime.datetime.today().strftime(
+            "capture-%Y-%m-%d-%H-%M-%S.png")
+        print("Capturing image as %s" % image_name)
+        cv2.imwrite(image_name, frame)
+
+# When everything's done, release the capture
+cap.release()
+cv2.destroyAllWindows()
diff --git a/y2020/vision/tools/python_code/image_match_test.py b/y2020/vision/tools/python_code/image_match_test.py
index d6b7deb..455bc30 100644
--- a/y2020/vision/tools/python_code/image_match_test.py
+++ b/y2020/vision/tools/python_code/image_match_test.py
@@ -1,36 +1,20 @@
 import cv2
 import math
-from matplotlib import pyplot as plt
 import numpy as np
 import time
 
 import field_display
 import train_and_match as tam
+import target_definition
+import camera_definition
 
 ### DEFINITIONS
+target_definition.USE_BAZEL = False
+target_list = target_definition.compute_target_definition()
+camera_list = camera_definition.camera_list
 
-# List of images to train on
-training_image_names = [
-    'test_images/train_power_port_blue.png',
-    # Using webcam captured image for testing-- best to use only one of the two
-    # training images for the power_port_red
-    'test_images/train_power_port_red_webcam.png',
-    #    'test_images/train_power_port_red.png',
-    'test_images/train_loading_bay_blue.png',
-    'test_images/train_loading_bay_red.png'
-]
-
-# Target points on the image -- needs to match training images-- one per 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
-target_pt_list = [
-    np.float32([[393, 147]]).reshape(-1, 1, 2),  # train_power_port_blue.png
-    np.float32([[305, 97]]).reshape(-1, 1, 2),  #train_power_port_red_webcam.png
-    #    np.float32([[570,192]]).reshape(-1,1,2), # train_power_port_red.png
-    np.float32([[366, 236]]).reshape(-1, 1, 2),  # train_loading_bay_blue.png
-    np.float32([[366, 236]]).reshape(-1, 1, 2),  # train_loading_bay_red.png
-]
+# For now, just use the first one
+camera_params = camera_list[0]
 
 # Put list of all possible images we want to test, and then we'll select one
 # Note, if you query on the same image as training, only that image will match
@@ -45,8 +29,8 @@
     'test_images/test_raspi3_sample.jpg',  #7
     'test_images/test_VR_sample1.png',  #8
     'test_images/train_loading_bay_blue.png',  #9
-    'test_images/train_loading_bay_red.png'
-]  #10
+    'test_images/train_loading_bay_red.png'  #10
+]
 
 training_image_index = 0
 # TODO: Should add argParser here to select this
@@ -54,15 +38,25 @@
 
 ##### Let's get to work!
 
-# Load the training and query images
-training_images = tam.load_images(training_image_names)
+training_image_names = []
+training_images = []
+train_keypoint_lists = []
+train_descriptor_lists = []
+target_points_tmp = []
+target_pt_list = None
+# Popluate the data structures used by this function
+for target in target_list:
+    # Image names
+    print("Target filename:", target.image_filename)
+    training_image_names.append(target.image_filename)
+    # The training images
+    training_images.append(target.image)
+    # Keypoints and desciptors
+    train_keypoint_lists.append(target.keypoint_list)
+    train_descriptor_lists.append(target.descriptor_list)
+    target_points_tmp.append(target.target_point_2d)
 
-# Create feature extractor
-feature_extractor = tam.load_feature_extractor()
-
-# Extract keypoints and descriptors for model
-train_keypoint_lists, train_descriptor_lists = tam.detect_and_compute(
-    feature_extractor, training_images)
+target_pt_list = np.asarray(target_points_tmp).reshape(-1, 1, 2)
 
 # # Create the matcher.  This only needs to be created once
 ts = time.monotonic()  # Do some basic timing
@@ -72,6 +66,11 @@
 for i in range(len(train_keypoint_lists)):
     print("Model ", i, " has ", len(train_keypoint_lists[i]), " features: ")
 
+# Create feature extractor
+# TODO<Jim>: Should probably make sure we're using the same extractor for
+# training and query
+feature_extractor = tam.load_feature_extractor()
+
 # Load the query image in.  Based on index in our list, or using camera if index is -1
 query_images = []
 
@@ -121,183 +120,160 @@
     for i in range(len(train_keypoint_lists)):
         print("Model ", i, " has # good matches: ", len(good_matches_list[i]))
 
-    # TEMP: Use first list for what follows
-    # TODO: Should look for best match after homography and display that one
-    # OR: show results on all good matches
-    good_matches = good_matches_list[0]
+    # If we're querying static images, show full results
+    if (query_image_index is not -1):
+        print("Showing results for static query image")
+        homography_list, matches_mask_list = tam.show_results(
+            training_images, train_keypoint_lists, query_images,
+            query_keypoint_lists, target_pt_list, good_matches_list)
 
     # Next, find homography (map between training and query images)
     homography_list, matches_mask_list = tam.compute_homographies(
         train_keypoint_lists, query_keypoint_lists, good_matches_list)
 
-    if matches_mask_list[0].count(1) < tam.MIN_MATCH_COUNT:
-        print(
-            "Not continuing pose calc because not enough good matches after homography-- only had ",
-            matches_mask_list[0].count(1))
-        continue
+    # Image used to store field plot
+    img_ret = None
+    for i, good_matches in enumerate(good_matches_list):
+        # TODO: Should look for best match after homography and display that one
+        # OR: show results on all good matches
+        print("Processing match for model %d" % i)
 
-    # Next, let's go to compute pose
-    # I've chosen some coordinate frames to help track things.  Using indices:
-    #   w:    world coordinate frame (at center wall of driver station)
-    #   tarj: camera pose when jth target (e.g., power panel) was captured
-    #   ci:   ith camera
-    #   b:    body frame of robot (robot location)
-    #
-    # And, T for translation, R for rotation, "est" for estimated
-    # So, T_w_b_est is the estimated translation from world to body coords
-    #
-    # A few notes:
-    #   -- CV uses rotated frame, where z points out of camera, y points down
-    #   -- This causes a lot of mapping around of points, but overall OK
+        if matches_mask_list[i].count(1) < tam.MIN_MATCH_COUNT:
+            print(
+                "Not continuing pose calc because not enough good matches after homography-- only had ",
+                matches_mask_list[i].count(1))
+            continue
 
-    ### TODO: Still need to clean this rest up
+        # Next, let's go to compute pose
+        # I've chosen some coordinate frames to help track things.  Using indices:
+        #   w:    world coordinate frame (at center wall of driver station)
+        #   tarj: camera pose when jth target (e.g., power panel) was captured
+        #   ci:   ith camera
+        #   b:    body frame of robot (robot location)
+        #
+        # And, T for translation, R for rotation, "est" for estimated
+        # So, T_w_b_est is the estimated translation from world to body coords
+        #
+        # A few notes:
+        #   -- CV uses rotated frame, where z points out of camera, y points down
+        #   -- This causes a lot of mapping around of points, but overall OK
 
-    # TODO: Need to generalize this for each target-- need to have 3D global locations of each target's keypoints
-    field_length = 15.98
-    target_center_offset_y = 1.67
-    target_center_height = 2.49
-    if training_image_index is 0:
-        # webcam
-        fx = 810.
-        fy = 810.
-        cx = 320.
-        cy = 240.
-        cy_above_ground = 1.  # meters above ground
-        depth = 4.57  # meters
-        target_center_offset_y = 1.67
-    elif training_image_index is 1:
-        # Made up for screenshot from manual
-        cx = int(1165 / 2)
-        cy_above_ground = -0.5856  # meters above ground
-        cy = 776 / 2
-        fx = 1143  # pixels (or 0.00160m or 1143 pixels?)
-        depth = 4.57  # meters
-    elif training_image_index is 2:
-        cx = 732 / 2
-        cy_above_ground = 0.454  # meters above ground
-        cy = 436 / 2
-        target_width = 5 * 12 * 0.0254  # width in ft * in/ft * m/in
-        target_height = target_width * cy / cx  # width in ft * in/ft * m/in * cy / cx
-        FOV_x = 54 * math.pi / 180.  # camera FOV in radians (from 54 degree lens)
-        fx = cx / math.tan(FOV_x / 2)  # pixels
-        depth = (target_height / 2) / math.tan(FOV_x / 2)  # meters
+        src_pts_3d = []
+        for m in good_matches:
+            src_pts_3d.append(target_list[i].keypoint_list_3d[m.trainIdx])
+            pt = query_keypoint_lists[0][m.queryIdx].pt
+            print("Color at ", pt, " is ", query_images[0][int(pt[1])][int(
+                pt[0])])
+            query_images[0] = cv2.circle(
+                query_images[0], (int(pt[0]), int(pt[1])), 5, (0, 255, 0), 3)
 
-    src_pts_3d = []
-    for m in good_matches:
-        # Project the matches all back to 3D, assuming they're all at the same depth
-        # Add in the distance across the field and any lateral offset of the target
-        src_pts_3d.append([(
-            field_length,
-            -(train_keypoint_lists[training_image_index][m.trainIdx].pt[0] - cx
-              ) * depth / fx - target_center_offset_y,
-            -(train_keypoint_lists[training_image_index][m.trainIdx].pt[1] - cy
-              ) * depth / fx + cy_above_ground)])
+        cv2.imshow('DEBUG', query_images[0]), cv2.waitKey(0)
+        # Reshape 3d point list to work with computations to follow
+        src_pts_3d_array = np.asarray(np.float32(src_pts_3d)).reshape(-1, 3, 1)
 
-    # Reshape 3d point list to work with computations to follow
-    src_pts_3d_array = np.asarray(np.float32(src_pts_3d)).reshape(-1, 3, 1)
+        # Load from camera parameters
+        cam_mat = camera_params.camera_int.camera_matrix
+        dist_coeffs = camera_params.camera_int.dist_coeffs
 
-    cam_mat = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
-    distortion_coeffs = np.zeros((5, 1))
+        # Create list of matching query point locations
+        dst_pts = np.float32([
+            query_keypoint_lists[0][m.queryIdx].pt for m in good_matches
+        ]).reshape(-1, 1, 2)
 
-    # Create list of matching query point locations
-    dst_pts = np.float32([
-        query_keypoint_lists[0][m.queryIdx].pt for m in good_matches
-    ]).reshape(-1, 1, 2)
+        ts = time.monotonic()
+        # TODO: May want to supply it with estimated guess as starting point
+        # Find offset from camera to original location of camera relative to target
+        retval, R_ci_w_estj, T_ci_w_estj, inliers = cv2.solvePnPRansac(
+            src_pts_3d_array, dst_pts, cam_mat, dist_coeffs)
 
-    ts = time.monotonic()
-    # TODO: May want to supply it with estimated guess as starting point
-    # Find offset from camera to original location of camera relative to target
-    retval, R_ci_w_estj, T_ci_w_estj, inliers = cv2.solvePnPRansac(
-        src_pts_3d_array, dst_pts, cam_mat, distortion_coeffs)
+        tf = time.monotonic()
+        print("Solving Pose took ", tf - ts, " secs")
+        print("Found ", len(inliers), " inliers, out of ", len(dst_pts),
+              " matched points")
+        ### THIS is the estimate of the robot on the field!
+        R_w_ci_estj, T_w_ci_estj = field_display.invert_pose_Rodrigues(
+            R_ci_w_estj, T_ci_w_estj)
+        #print("Pose from PnP is:\n", R_ci_w_estj, "\n", T_ci_w_estj)
+        # The rotation estimate is for camera with z pointing forwards.
+        # Compute the rotation as if x is forward
+        R_w_ci_estj_robot = field_display.camera_rot_to_world_Rodrigues(
+            R_w_ci_estj)
+        #print("Pose in world coords is:\n", R_w_ci_estj, "\n", T_w_ci_estj,
+        #      "\nWith robot coord frame rotation as \n", R_w_ci_estj_robot)
 
-    tf = time.monotonic()
-    print("Solving Pose took ", tf - ts, " secs")
-    print("Found ", len(inliers), " inliers, out of ", len(dst_pts),
-          " matched points")
-    ### THIS is the estimate of the robot on the field!
-    R_w_ci_estj, T_w_ci_estj = field_display.invert_pose_Rodrigues(
-        R_ci_w_estj, T_ci_w_estj)
-    print("Pose from PnP is:\n", R_ci_w_estj, "\n", T_ci_w_estj)
-    # The rotation estimate is for camera with z pointing forwards.
-    # Compute the rotation as if x is forward
-    R_w_ci_estj_robot = field_display.camera_rot_to_world_Rodrigues(
-        R_w_ci_estj)
-    print("Pose in world coords is:\n", R_w_ci_estj, "\n", T_w_ci_estj,
-          "\nWith robot coord frame rotation as \n", R_w_ci_estj_robot)
+        # Use the rotational component about the z axis to define the heading
+        # TODO<jim>: Change this to use rotation of x-unit vector
+        heading_est = R_w_ci_estj_robot[2][0]
+        # Plot location of the robot, along with heading
+        color = (255, 0, 0)  # Blue
+        if i in (0, 1):
+            color = (0, 0, 255)  # Red
+        img_ret = field_display.plot_bot_on_field(img_ret, color, T_w_ci_estj,
+                                                  heading_est)
 
-    # Use the rotational component about the z axis to define the heading
-    heading_est = R_w_ci_estj_robot[2][0]
-    # Plot location of the robot, along with heading
-    img_ret = field_display.plot_bot_on_field(None, (0, 255, 255), T_w_ci_estj,
-                                              heading_est)
+        # Compute vector to target
+        T_w_target_pt = np.array(target_list[i].target_position).reshape(3, 1)
+        vector_to_target = T_w_target_pt - T_w_ci_estj
+        # Compute distance to target (full 3D)
+        distance_to_target = np.linalg.norm(vector_to_target)
+        # Compute distance to target (x,y)
+        distance_to_target_ground = np.linalg.norm(vector_to_target[0:2])
+        #print("Distance comparison: all: ", distance_to_target, " vs. ground: ", distance_to_target_ground)
 
-    # Compute vector to target
-    T_w_target_pt = np.array(
-        [[field_length, -target_center_offset_y, target_center_height]]).T
-    vector_to_target = T_w_target_pt - T_w_ci_estj
-    # Compute distance to target (full 3D)
-    distance_to_target = np.linalg.norm(vector_to_target)
-    # Compute distance to target (x,y)
-    distance_to_target_ground = np.linalg.norm(vector_to_target[0:2])
-    #print("Distance comparison: all: ", distance_to_target, " vs. ground: ", distance_to_target_ground)
+        angle_to_target_abs = math.atan2(vector_to_target[1][0],
+                                         vector_to_target[0][0])
+        angle_to_target_robot = angle_to_target_abs - heading_est
+        img_ret = field_display.plot_line_on_field(img_ret, (255, 255, 0),
+                                                   T_w_ci_estj, T_w_target_pt)
+        # THESE ARE OUR ESTIMATES OF HEADING, DISTANCE, AND SKEW TO TARGET
+        log_file.write('%lf, %lf, %lf\n' %
+                       (heading_est, distance_to_target_ground,
+                        angle_to_target_robot))
 
-    angle_to_target_abs = math.atan2(vector_to_target[1][0],
-                                     vector_to_target[0][0])
-    angle_to_target_robot = angle_to_target_abs - heading_est
-    img_ret = field_display.plot_line_on_field(img_ret, (255, 255, 0),
-                                               T_w_ci_estj, T_w_target_pt)
-    # THESE ARE OUR ESTIMATES OF HEADING, DISTANCE, AND SKEW TO TARGET
-    log_file.write('%lf, %lf, %lf\n' % (heading_est, distance_to_target_ground,
-                                        angle_to_target_robot))
+        # A bunch of code to visualize things...
+        #
+        # Draw target on the image
+        h, w, ch = query_images[0].shape
+        query_image_copy = query_images[0].copy()
+        # Map target_pt onto image, for display
+        target_point_2d_trans = cv2.perspectiveTransform(
+            target_pt_list[i].reshape(-1, 1, 2),
+            homography_list[i]).astype(int)
+        # Ballpark the size of the circle so it looks right on image
+        radius = int(
+            32 * abs(homography_list[i][0][0] + homography_list[i][1][1]) /
+            2)  # Average of scale factors
+        query_image_copy = cv2.circle(query_image_copy,
+                                      (target_point_2d_trans.flatten()[0],
+                                       target_point_2d_trans.flatten()[1]),
+                                      radius, (0, 255, 0), 3)
 
-    # A bunch of code to visualize things...
-    #
-    # Draw target on the image
-    h, w, ch = query_images[0].shape
-    query_image_copy = query_images[0].copy()
-    # Map target_pt onto image, for display
-    transformed_target = cv2.perspectiveTransform(target_pt_list[0],
-                                                  homography_list[0])
-    # Ballpark the size of the circle so it looks right on image
-    radius = int(32 * abs(homography_list[0][0][0] + homography_list[0][1][1])
-                 / 2)  # Average of scale factors
-    query_image_copy = cv2.circle(
-        query_image_copy,
-        (transformed_target.flatten()[0], transformed_target.flatten()[1]),
-        radius, (0, 255, 0), 3)
+        # Create empty image the size of the field
+        img_heading = field_display.load_field_image()
+        img_heading[:, :, :] = 0
+        f_h, f_w, f_ch = img_heading.shape
 
-    # If we're querying static images, show full results
-    if (query_image_index is not -1):
-        homography_list, matches_mask_list = tam.show_results(
-            training_images, train_keypoint_lists, query_images,
-            query_keypoint_lists, target_pt_list, good_matches_list)
+        # Create heading view, and paste it to bottom of field
+        img_heading = field_display.plot_camera_to_target(
+            img_heading, (0, 255, 0), heading_est, distance_to_target_ground,
+            angle_to_target_robot)
+        vis = np.concatenate((img_ret, img_heading), axis=0)
 
-    # Create empty image the size of the field
-    img_heading = field_display.load_field_image()
-    img_heading[:, :, :] = 0
-    f_h, f_w, f_ch = img_heading.shape
+        # Paste query image to right of other views (scale to keep aspect ratio)
+        img_query_scaled = cv2.resize(query_image_copy,
+                                      (int(2 * w * f_h / h), 2 * f_h))
+        vis = np.concatenate((vis, img_query_scaled), axis=1)
 
-    # Create heading view, and paste it to bottom of field
-    img_heading = field_display.plot_camera_to_target(
-        img_heading, (0, 255, 0), heading_est, distance_to_target_ground,
-        angle_to_target_robot)
-    vis = np.concatenate((img_ret, img_heading), axis=0)
+        # Scale down to make it fit on screen
+        vis = cv2.resize(vis, (int(vis.shape[1] / 4), int(vis.shape[0] / 4)))
+        cv2.imshow('field_display', vis)
 
-    # Paste query image to right of other views (scale to keep aspect ratio)
-    img_query_scaled = cv2.resize(query_image_copy,
-                                  (int(2 * w * f_h / h), 2 * f_h))
-    vis = np.concatenate((vis, img_query_scaled), axis=1)
-
-    # Scale down to make it fit on screen
-    vis = cv2.resize(vis, (int(vis.shape[1] / 4), int(vis.shape[0] / 4)))
-    cv2.imshow('field_display', vis)
-
-    #Waits for a user input to quit the application
-    pause_time = 0
-    if (query_image_index is -1):
-        pause_time = 1
-    if cv2.waitKey(pause_time) & 0xFF == ord('q'):
-        break
+        #Waits for a user input to quit the application
+        pause_time = 0
+        if (query_image_index is -1):
+            pause_time = 1
+        if cv2.waitKey(pause_time) & 0xFF == ord('q'):
+            break
 
 # Done.  Clean things up
 if (query_image_index is -1):
@@ -305,4 +281,3 @@
     cap.release()
 
 cv2.destroyAllWindows()
-
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 9fa4acf..0ce70b7 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -1,100 +1,232 @@
 #!/usr/bin/python3
 
 import cv2
+import glog
+import numpy as np
 import sys
 import flatbuffers
-import target_definition
 
-import frc971.vision.sift.TrainingImage as TrainingImage
-import frc971.vision.sift.TrainingData as TrainingData
+import frc971.vision.sift.CameraCalibration as CameraCalibration
 import frc971.vision.sift.Feature as Feature
+import frc971.vision.sift.KeypointFieldLocation as KeypointFieldLocation
+import frc971.vision.sift.TrainingData as TrainingData
+import frc971.vision.sift.TrainingImage as TrainingImage
+import frc971.vision.sift.TransformationMatrix as TransformationMatrix
+
+
+# Takes a 3x3 rotation matrix and 3x1 translation vector, and outputs 12
+# element list, suitable for outputing to flatbuffer
+def rot_and_trans_to_list(R, T):
+    output_list = []
+    for row in range(3):
+        for col in range(3):
+            output_list.append(R[row][col])
+        output_list.append(T[row])
+
+    output_list = output_list + [0., 0., 0., 1.]
+    return output_list
+
 
 def main():
 
-  output_path = sys.argv[1]
-  print("Writing file to ", output_path)
+    target_data_list = None
+    camera_calib_list = None
 
-  target_data_list = target_definition.compute_target_definition()
+    output_path = sys.argv[1]
 
-  fbb = flatbuffers.Builder(0)
+    if (len(sys.argv) > 2):
+        if sys.argv[2] == "test":
+            glog.info("Loading test data")
+            import camera_definition_test
+            import target_definition_test
+            target_data_list = target_definition_test.compute_target_definition(
+            )
+            camera_calib_list = camera_definition_test.camera_list
+        else:
+            glog.error("Unhandled arguments: '%s'" % sys.argv[2])
+            quit()
+    else:
+        glog.info("Loading target configuration data")
+        import camera_definition
+        import target_definition
+        target_data_list = target_definition.compute_target_definition()
+        camera_calib_list = camera_definition.camera_list
 
-  images_vector = []
+    glog.info("Writing file to ", output_path)
 
-  for target_data in target_data_list:
+    fbb = flatbuffers.Builder(0)
 
-    features_vector = []
+    images_vector = []
 
-    for keypoint, keypoint_3d, descriptor in zip(target_data.keypoint_list,
-                                                 target_data.keypoint_list_3d,
-                                                 target_data.descriptor_list):
+    # Iterate overall the training targets
+    for target_data in target_data_list:
 
-      Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
-      for n in reversed(descriptor):
-        fbb.PrependFloat32(n)
-      descriptor_vector = fbb.EndVector(len(descriptor))
+        features_vector = []
 
-      Feature.FeatureStart(fbb)
+        # Iterate over all the keypoints
+        for keypoint, keypoint_3d, descriptor in zip(
+                target_data.keypoint_list, target_data.keypoint_list_3d,
+                target_data.descriptor_list):
 
-      Feature.FeatureAddDescriptor(fbb, descriptor_vector)
-      Feature.FeatureAddX(fbb, keypoint.pt[0])
-      Feature.FeatureAddY(fbb, keypoint.pt[1])
-      Feature.FeatureAddSize(fbb, keypoint.size)
-      Feature.FeatureAddAngle(fbb, keypoint.angle)
-      Feature.FeatureAddResponse(fbb, keypoint.response)
-      Feature.FeatureAddOctave(fbb, keypoint.octave)
+            # Build the Descriptor vector
+            Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
+            for n in reversed(descriptor):
+                assert n == round(n)
+                fbb.PrependUint8(int(round(n)))
+            descriptor_vector = fbb.EndVector(len(descriptor))
 
-      features_vector.append(Feature.FeatureEnd(fbb))
+            # Add all the components to the each Feature
+            Feature.FeatureStart(fbb)
+            Feature.FeatureAddDescriptor(fbb, descriptor_vector)
+            Feature.FeatureAddX(fbb, keypoint.pt[0])
+            Feature.FeatureAddY(fbb, keypoint.pt[1])
+            Feature.FeatureAddSize(fbb, keypoint.size)
+            Feature.FeatureAddAngle(fbb, keypoint.angle)
+            Feature.FeatureAddResponse(fbb, keypoint.response)
+            Feature.FeatureAddOctave(fbb, keypoint.octave)
 
-      ## TODO: Write 3d vector here
+            keypoint_3d_location = KeypointFieldLocation.CreateKeypointFieldLocation(
+                fbb, keypoint_3d[0][0], keypoint_3d[0][1], keypoint_3d[0][2])
 
-    TrainingImage.TrainingImageStartFeaturesVector(fbb, len(features_vector))
-    for feature in reversed(features_vector):
-      fbb.PrependUOffsetTRelative(feature)
-    features_vector_table = fbb.EndVector(len(features_vector))
+            Feature.FeatureAddFieldLocation(fbb, keypoint_3d_location)
 
-    TrainingImage.TrainingImageStart(fbb)
-    TrainingImage.TrainingImageAddFeatures(fbb, features_vector_table)
-    # TODO(Brian): Fill out the transformation matrices.
-    images_vector.append(TrainingImage.TrainingImageEnd(fbb))
+            features_vector.append(Feature.FeatureEnd(fbb))
 
-  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))
+        # Create the field_to_target TransformationMatrix
+        field_to_target_list = rot_and_trans_to_list(
+            target_data.target_rotation, target_data.target_position)
+        TransformationMatrix.TransformationMatrixStartDataVector(
+            fbb, len(field_to_target_list))
+        for n in reversed(field_to_target_list):
+            fbb.PrependFloat32(n)
+        field_to_target_offset = fbb.EndVector(len(field_to_target_list))
 
-  TrainingData.TrainingDataStart(fbb)
-  TrainingData.TrainingDataAddImages(fbb, images_vector_table)
-  fbb.Finish(TrainingData.TrainingDataEnd(fbb))
+        TransformationMatrix.TransformationMatrixStart(fbb)
+        TransformationMatrix.TransformationMatrixAddData(
+            fbb, field_to_target_offset)
+        transformation_mat_offset = TransformationMatrix.TransformationMatrixEnd(
+            fbb)
 
-  bfbs = fbb.Output()
+        # Create the TrainingImage feature vector
+        TrainingImage.TrainingImageStartFeaturesVector(fbb,
+                                                       len(features_vector))
+        for feature in reversed(features_vector):
+            fbb.PrependUOffsetTRelative(feature)
+        features_vector_offset = fbb.EndVector(len(features_vector))
 
-  output_prefix = [
-      b'#ifndef Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
-      b'#define Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
-      b'#include <string_view>',
-      b'namespace frc971 {',
-      b'namespace vision {',
-      b'inline std::string_view SiftTrainingData() {',
-  ]
-  output_suffix = [
-      b'  return std::string_view(kData, sizeof(kData));',
-      b'}',
-      b'}  // namespace vision',
-      b'}  // namespace frc971',
-      b'#endif  // Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
-  ]
+        # Add the TrainingImage data
+        TrainingImage.TrainingImageStart(fbb)
+        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])
+        TrainingImage.TrainingImageAddTargetPointRadius(
+            fbb, target_data.target_radius)
+        images_vector.append(TrainingImage.TrainingImageEnd(fbb))
 
-  with open(output_path, 'wb') as output:
-    for line in output_prefix:
-      output.write(line)
-      output.write(b'\n')
-    output.write(b'alignas(64) static constexpr char kData[] = "')
-    for byte in fbb.Output():
-      output.write(b'\\x' + (b'%x' % byte).zfill(2))
-    output.write(b'";\n')
-    for line in output_suffix:
-      output.write(line)
-      output.write(b'\n')
+    # Create and add Training Data of all targets
+    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))
+
+    # Create camera calibration data
+    camera_calibration_vector = []
+    for camera_calib in camera_calib_list:
+        fixed_extrinsics_list = rot_and_trans_to_list(
+            camera_calib.camera_ext.R, camera_calib.camera_ext.T)
+        TransformationMatrix.TransformationMatrixStartDataVector(
+            fbb, len(fixed_extrinsics_list))
+        for n in reversed(fixed_extrinsics_list):
+            fbb.PrependFloat32(n)
+        fixed_extrinsics_data_offset = fbb.EndVector(
+            len(fixed_extrinsics_list))
+
+        TransformationMatrix.TransformationMatrixStart(fbb)
+        TransformationMatrix.TransformationMatrixAddData(
+            fbb, fixed_extrinsics_data_offset)
+        fixed_extrinsics_vector = TransformationMatrix.TransformationMatrixEnd(
+            fbb)
+
+        # TODO: Need to add in distortion coefficients here
+        # For now, just send camera paramter matrix (fx, fy, cx, cy)
+        camera_int_list = camera_calib.camera_int.camera_matrix.ravel().tolist(
+        )
+        CameraCalibration.CameraCalibrationStartIntrinsicsVector(
+            fbb, len(camera_int_list))
+        for n in reversed(camera_int_list):
+            fbb.PrependFloat32(n)
+        intrinsics_vector = fbb.EndVector(len(camera_int_list))
+
+        dist_coeffs_list = camera_calib.camera_int.dist_coeffs.ravel().tolist()
+        CameraCalibration.CameraCalibrationStartDistCoeffsVector(
+            fbb, len(dist_coeffs_list))
+        for n in reversed(dist_coeffs_list):
+            fbb.PrependFloat32(n)
+        dist_coeffs_vector = fbb.EndVector(len(dist_coeffs_list))
+
+        node_name_offset = fbb.CreateString(camera_calib.node_name)
+        CameraCalibration.CameraCalibrationStart(fbb)
+        CameraCalibration.CameraCalibrationAddNodeName(fbb, node_name_offset)
+        CameraCalibration.CameraCalibrationAddTeamNumber(
+            fbb, camera_calib.team_number)
+        CameraCalibration.CameraCalibrationAddIntrinsics(
+            fbb, intrinsics_vector)
+        CameraCalibration.CameraCalibrationAddDistCoeffs(
+            fbb, dist_coeffs_vector)
+        CameraCalibration.CameraCalibrationAddFixedExtrinsics(
+            fbb, fixed_extrinsics_vector)
+        camera_calibration_vector.append(
+            CameraCalibration.CameraCalibrationEnd(fbb))
+
+    TrainingData.TrainingDataStartCameraCalibrationsVector(
+        fbb, len(camera_calibration_vector))
+    for camera_calibration in reversed(camera_calibration_vector):
+        fbb.PrependUOffsetTRelative(camera_calibration)
+    camera_calibration_vector_table = fbb.EndVector(
+        len(camera_calibration_vector))
+
+    # Fill out TrainingData
+    TrainingData.TrainingDataStart(fbb)
+    TrainingData.TrainingDataAddImages(fbb, images_vector_table)
+    TrainingData.TrainingDataAddCameraCalibrations(
+        fbb, camera_calibration_vector_table)
+    fbb.Finish(TrainingData.TrainingDataEnd(fbb))
+
+    bfbs = fbb.Output()
+
+    output_prefix = [
+        b'#ifndef Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
+        b'#define Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
+        b'#include <string_view>',
+        b'namespace frc971 {',
+        b'namespace vision {',
+        b'inline std::string_view SiftTrainingData() {',
+    ]
+    output_suffix = [
+        b'  return std::string_view(kData, sizeof(kData));',
+        b'}',
+        b'}  // namespace vision',
+        b'}  // namespace frc971',
+        b'#endif  // Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
+    ]
+
+    # Write out the header file
+    with open(output_path, 'wb') as output:
+        for line in output_prefix:
+            output.write(line)
+            output.write(b'\n')
+        output.write(b'alignas(64) static constexpr char kData[] = "')
+        for byte in fbb.Output():
+            output.write(b'\\x' + (b'%x' % byte).zfill(2))
+        output.write(b'";\n')
+        for line in output_suffix:
+            output.write(line)
+            output.write(b'\n')
+
 
 if __name__ == '__main__':
-  main()
+    main()
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index 779eb0b..430e83f 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -1,6 +1,7 @@
 import argparse
 import cv2
-import json
+# TODO<Jim>: Add gflags for handling command-line flags
+import glog
 import math
 import numpy as np
 
@@ -8,11 +9,17 @@
 import define_training_data as dtd
 import train_and_match as tam
 
+# TODO<Jim>: Allow command-line setting of logging level
+glog.setLevel("WARN")
 global VISUALIZE_KEYPOINTS
 global USE_BAZEL
 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
     if USE_BAZEL:
@@ -20,6 +27,7 @@
 
     return ret_name
 
+
 class TargetData:
     def __init__(self, filename):
         self.image_filename = filename
@@ -27,7 +35,8 @@
         if USE_BAZEL:
             from bazel_tools.tools.python.runfiles import runfiles
             r = runfiles.Create()
-            self.image_filename = r.Rlocation(bazel_name_fix(self.image_filename))
+            self.image_filename = r.Rlocation(
+                bazel_name_fix(self.image_filename))
         self.image = tam.load_images([self.image_filename])[0]
         self.polygon_list = []
         self.polygon_list_3d = []
@@ -35,8 +44,15 @@
         self.keypoint_list = []
         self.keypoint_list_3d = None  # numpy array of 3D points
         self.descriptor_list = []
+        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):
+    def extract_features(self, feature_extractor=None):
+        if feature_extractor is None:
+            feature_extractor = tam.load_feature_extractor()
+
         kp_lists, desc_lists = tam.detect_and_compute(feature_extractor,
                                                       [self.image])
         self.keypoint_list = kp_lists[0]
@@ -61,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]])
-            print("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)
@@ -73,6 +89,7 @@
 
         return point_list_3d
 
+
 def compute_target_definition():
     ############################################################
     # TARGET DEFINITIONS
@@ -81,180 +98,261 @@
     ideal_target_list = []
     training_target_list = []
 
-    # Some general info about our field and targets
-    # Assume camera centered on target at 1 m above ground and distance of 4.85m
-    field_length = 15.98
+    # Using coordinate system as defined in sift.fbs:
+    # field origin (0, 0, 0) at floor height at center of field
+    # Robot orientation with x-axis pointing towards RED ALLIANCE player station
+    # y-axis to left and z-axis up (right-hand coordinate system)
+    # Thus, the red power port target location will be at (-15.98/2,1.67,0),
+    # with orientation (facing out of the field) of M_PI
+
+    # Field constants
+    field_length = 15.983
     power_port_total_height = 3.10
-    power_port_center_y = 1.67
-    power_port_width = 1.22
-    power_port_bottom_wing_height = 1.88
+    power_port_edge_y = 1.089
+    power_port_width = 1.225
+    power_port_bottom_wing_height = 1.828
     power_port_wing_width = 1.83
-    loading_bay_edge_y = 1.11
-    loading_bay_width = 1.52
-    loading_bay_height = 0.94
+    loading_bay_edge_y = 0.784
+    loading_bay_width = 1.524
+    loading_bay_height = 0.933
+    wing_angle = 20. * math.pi / 180.
 
     # Pick the target center location at halfway between top and bottom of the top panel
-    target_center_height = (power_port_total_height + power_port_bottom_wing_height) / 2.
-
-    # TODO: Still need to figure out what this angle actually is
-    wing_angle = 20. * math.pi / 180.
+    power_port_target_height = (
+        power_port_total_height + power_port_bottom_wing_height) / 2.
 
     ###
     ### Red Power Port
     ###
 
     # Create the reference "ideal" image
-    ideal_power_port_red = TargetData('test_images/train_power_port_red.png')
+    ideal_power_port_red = TargetData('test_images/ideal_power_port_red.png')
 
     # Start at lower left corner, and work around clockwise
     # These are taken by manually finding the points in gimp for this image
     power_port_red_main_panel_polygon_points_2d = [(451, 679), (451, 304),
-                                                   (100, 302), (451, 74),
-                                                   (689, 74), (689, 302),
-                                                   (689, 679)]
+                                                   (100, 302), (451, 74), (689,
+                                                                           74),
+                                                   (689, 302), (689, 679)]
 
     # These are "virtual" 3D points based on the expected geometry
     power_port_red_main_panel_polygon_points_3d = [
-        (field_length, -power_port_center_y + power_port_width / 2., 0.),
-        (field_length, -power_port_center_y + power_port_width / 2.,
-         power_port_bottom_wing_height),
-        (field_length, -power_port_center_y + power_port_width / 2.
-         + power_port_wing_width, power_port_bottom_wing_height),
-        (field_length, -power_port_center_y + power_port_width / 2.,
-         power_port_total_height),
-        (field_length, -power_port_center_y - power_port_width / 2.,
-         power_port_total_height),
-        (field_length, -power_port_center_y - power_port_width / 2.,
-         power_port_bottom_wing_height),
-        (field_length, -power_port_center_y - power_port_width / 2., 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, -power_port_center_y - power_port_width / 2.,
+        (-field_length / 2., power_port_edge_y + power_port_width,
          power_port_total_height),
-        (field_length - power_port_wing_width * math.sin(wing_angle),
-         -power_port_center_y - power_port_width / 2.
-         - power_port_wing_width * math.cos(wing_angle),
-         power_port_bottom_wing_height),
-        (field_length, -power_port_center_y - power_port_width / 2.,
-         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
-    ideal_power_port_red.polygon_list.append(power_port_red_main_panel_polygon_points_2d)
-    ideal_power_port_red.polygon_list_3d.append(power_port_red_main_panel_polygon_points_3d)
+    ideal_power_port_red.polygon_list.append(
+        power_port_red_main_panel_polygon_points_2d)
+    ideal_power_port_red.polygon_list_3d.append(
+        power_port_red_main_panel_polygon_points_3d)
 
-    ideal_power_port_red.polygon_list.append(power_port_red_wing_panel_polygon_points_2d)
-    ideal_power_port_red.polygon_list_3d.append(power_port_red_wing_panel_polygon_points_3d)
+    # Define the pose of the target
+    # Location is on the ground, at the center of the target
+    # Orientation is with "x" pointing out of the field, and "z" up
+    # This way, if robot is facing target directly, the x-axes are aligned
+    # 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
+    ])
+
+    # 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)  # ideal_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
     ideal_target_list.append(ideal_power_port_red)
     # And add the training image we'll actually use to the training list
-    training_target_list.append(TargetData('test_images/train_power_port_red_webcam.png'))
+    training_target_power_port_red = TargetData(
+        'test_images/train_power_port_red_webcam.png')
+    #'test_images/train_power_port_red_pi-7971-3.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)
 
     ###
     ### Red Loading Bay
     ###
 
-    ideal_loading_bay_red = TargetData('test_images/train_loading_bay_red.png')
+    ideal_loading_bay_red = TargetData('test_images/ideal_loading_bay_red.png')
 
     # Start at lower left corner, and work around clockwise
     # These are taken by manually finding the points in gimp for this image
-    loading_bay_red_polygon_points_2d = [(42, 406), (42, 35), (651, 34), (651, 406)]
+    loading_bay_red_polygon_points_2d = [(42, 406), (42, 35), (651, 34), (651,
+                                                                          406)]
 
     # These are "virtual" 3D points based on the expected geometry
     loading_bay_red_polygon_points_3d = [
-        (field_length, loading_bay_edge_y + loading_bay_width, 0.),
-        (field_length, loading_bay_edge_y + loading_bay_width, loading_bay_height),
-        (field_length, loading_bay_edge_y, loading_bay_height),
-        (field_length, 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(loading_bay_red_polygon_points_2d)
-    ideal_loading_bay_red.polygon_list_3d.append(loading_bay_red_polygon_points_3d)
+    ideal_loading_bay_red.polygon_list.append(
+        loading_bay_red_polygon_points_2d)
+    ideal_loading_bay_red.polygon_list_3d.append(
+        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)  # ideal_loading_bay_red.png
 
     ideal_target_list.append(ideal_loading_bay_red)
-    training_target_list.append(TargetData('test_images/train_loading_bay_red.png'))
+    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)
 
     ###
     ### Blue Power Port
     ###
 
-    ideal_power_port_blue = TargetData('test_images/train_power_port_blue.png')
+    ideal_power_port_blue = TargetData('test_images/ideal_power_port_blue.png')
 
     # Start at lower left corner, and work around clockwise
     # These are taken by manually finding the points in gimp for this image
     power_port_blue_main_panel_polygon_points_2d = [(438, 693), (438, 285),
-                                                    (93, 285), (440, 50),
-                                                    (692, 50), (692, 285),
-                                                    (692, 693)]
+                                                    (93, 285), (440, 50), (692,
+                                                                           50),
+                                                    (692, 285), (692, 693)]
 
     # These are "virtual" 3D points based on the expected geometry
     power_port_blue_main_panel_polygon_points_3d = [
-        (0., power_port_center_y - power_port_width / 2., 0.),
-        (0., power_port_center_y - power_port_width / 2.,
-         power_port_bottom_wing_height),
-        (0., power_port_center_y - power_port_width / 2. - power_port_wing_width,
-         power_port_bottom_wing_height),
-        (0., power_port_center_y - power_port_width / 2.,
-         power_port_total_height),
-        (0., power_port_center_y + power_port_width / 2.,
-         power_port_total_height),
-        (0., power_port_center_y + power_port_width / 2.,
-         power_port_bottom_wing_height),
-        (0., power_port_center_y + power_port_width / 2., 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 = [
-        (0., power_port_center_y + power_port_width / 2.,
+        (field_length / 2., -power_port_edge_y - power_port_width,
          power_port_total_height),
-        (power_port_wing_width * math.sin(wing_angle),
-         power_port_center_y - power_port_width / 2. +
+        (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),
-        (0., power_port_center_y + power_port_width / 2.,
+        (field_length / 2., -power_port_edge_y - power_port_width,
          power_port_bottom_wing_height)
     ]
 
     # Populate the blue power port
-    ideal_power_port_blue.polygon_list.append(power_port_blue_main_panel_polygon_points_2d)
-    ideal_power_port_blue.polygon_list_3d.append(power_port_blue_main_panel_polygon_points_3d)
+    ideal_power_port_blue.polygon_list.append(
+        power_port_blue_main_panel_polygon_points_2d)
+    ideal_power_port_blue.polygon_list_3d.append(
+        power_port_blue_main_panel_polygon_points_3d)
 
-    ideal_power_port_blue.polygon_list.append(power_port_blue_wing_panel_polygon_points_2d)
-    ideal_power_port_blue.polygon_list_3d.append(power_port_blue_wing_panel_polygon_points_3d)
+    # 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)  # ideal_power_port_blue.png
 
     ideal_target_list.append(ideal_power_port_blue)
-    training_target_list.append(TargetData('test_images/train_power_port_blue.png'))
+    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)
 
     ###
     ### Blue Loading Bay
     ###
 
-    ideal_loading_bay_blue = TargetData('test_images/train_loading_bay_blue.png')
+    ideal_loading_bay_blue = TargetData(
+        'test_images/ideal_loading_bay_blue.png')
 
     # Start at lower left corner, and work around clockwise
     # These are taken by manually finding the points in gimp for this image
-    loading_bay_blue_polygon_points_2d = [(7, 434), (7, 1), (729, 1), (729, 434)]
+    loading_bay_blue_polygon_points_2d = [(7, 434), (7, 1), (729, 1), (729,
+                                                                       434)]
 
     # These are "virtual" 3D points based on the expected geometry
     loading_bay_blue_polygon_points_3d = [
-        (field_length, loading_bay_edge_y + loading_bay_width, 0.),
-        (field_length, loading_bay_edge_y + loading_bay_width, loading_bay_height),
-        (field_length, loading_bay_edge_y, loading_bay_height),
-        (field_length, 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(loading_bay_blue_polygon_points_2d)
-    ideal_loading_bay_blue.polygon_list_3d.append(loading_bay_blue_polygon_points_3d)
+    ideal_loading_bay_blue.polygon_list.append(
+        loading_bay_blue_polygon_points_2d)
+    ideal_loading_bay_blue.polygon_list_3d.append(
+        loading_bay_blue_polygon_points_3d)
+
+    # 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)  # ideal_loading_bay_blue.png
 
     ideal_target_list.append(ideal_loading_bay_blue)
-    training_target_list.append(TargetData('test_images/train_loading_bay_blue.png'))
+    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
     feature_extractor = tam.load_feature_extractor()
@@ -263,7 +361,8 @@
     camera_params = camera_definition.web_cam_params
 
     for ideal_target in ideal_target_list:
-        print("\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()
@@ -272,16 +371,21 @@
 
         if VISUALIZE_KEYPOINTS:
             for i in range(len(ideal_target.polygon_list)):
-                ideal_pts_tmp = np.asarray(ideal_target.polygon_list[i]).reshape(
-                    -1, 2)
+                ideal_pts_tmp = np.asarray(
+                    ideal_target.polygon_list[i]).reshape(-1, 2)
                 ideal_pts_3d_tmp = np.asarray(
                     ideal_target.polygon_list_3d[i]).reshape(-1, 3)
                 # We can only compute pose if we have at least 4 points
                 # Only matters for reprojection for visualization
                 # Keeping this code here, since it's helpful when testing
                 if (len(ideal_target.polygon_list[i]) >= 4):
-                    img_copy = dtd.draw_polygon(ideal_target.image.copy(), ideal_target.polygon_list[i], (0,255,0), True)
-                    dtd.visualize_reprojections(img_copy, ideal_pts_tmp, ideal_pts_3d_tmp, camera_params.camera_int.camera_matrix, camera_params.camera_int.distortion_coeffs)
+                    img_copy = dtd.draw_polygon(ideal_target.image.copy(),
+                                                ideal_target.polygon_list[i],
+                                                (0, 255, 0), True)
+                    dtd.visualize_reprojections(
+                        img_copy, ideal_pts_tmp, ideal_pts_3d_tmp,
+                        camera_params.camera_int.camera_matrix,
+                        camera_params.camera_int.dist_coeffs)
 
             for polygon in ideal_target.polygon_list:
                 img_copy = ideal_target.image.copy()
@@ -298,7 +402,7 @@
                     np.asarray(kp_in_poly2d).reshape(-1, 2),
                     np.asarray(kp_in_poly3d).reshape(
                         -1, 3), camera_params.camera_int.camera_matrix,
-                    camera_params.camera_int.distortion_coeffs)
+                    camera_params.camera_int.dist_coeffs)
 
     ###############
     ### Compute 3D points on actual training images
@@ -307,7 +411,9 @@
     AUTO_PROJECTION = True
 
     if AUTO_PROJECTION:
-        print("\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
 
@@ -316,35 +422,49 @@
             training_target = training_target_list[target_ind]
             ideal_target = ideal_target_list[target_ind]
 
-            print("\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)
 
             # Create matcher that we'll use to match with ideal
             matcher = tam.train_matcher([training_target.descriptor_list])
 
-            matches_list = tam.compute_matches(matcher,
-                                               [training_target.descriptor_list],
-                                               [ideal_target.descriptor_list])
+            matches_list = tam.compute_matches(
+                matcher, [training_target.descriptor_list],
+                [ideal_target.descriptor_list])
 
             homography_list, matches_mask_list = tam.compute_homographies(
                 [training_target.keypoint_list], [ideal_target.keypoint_list],
                 matches_list)
 
+            # Compute H_inv, since this maps points in ideal target to
+            # points in the training target
+            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)
-                H_inv = np.linalg.inv(homography_list[0])
+                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(ideal_pts_2d, H_inv)
+                transformed_polygon = cv2.perspectiveTransform(
+                    ideal_pts_2d, H_inv)
                 training_target.polygon_list.append(transformed_polygon)
 
-            print("Started with %d keypoints" % len(training_target.keypoint_list))
+            # 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)
+
+            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,
                 training_target.polygon_list)
-            print("After filtering by polygons, had %d keypoints" % len(
+            glog.info("After filtering by polygons, had %d keypoints" % len(
                 training_target.keypoint_list))
             if VISUALIZE_KEYPOINTS:
                 tam.show_keypoints(training_target.image,
@@ -383,32 +503,41 @@
                 img_copy = training_target.image.copy()
                 for polygon in training_target.polygon_list:
                     pts = polygon.astype(int).reshape(-1, 2)
-                    img_copy = dtd.draw_polygon(img_copy, pts,
-                                             (255, 0, 0), True)
-                kp_tmp = np.asarray([
-                    (kp.pt[0], kp.pt[1]) for kp in training_target.keypoint_list
-                ]).reshape(-1, 2)
+                    img_copy = dtd.draw_polygon(img_copy, pts, (255, 0, 0),
+                                                True)
+                kp_tmp = np.asarray(
+                    [(kp.pt[0], kp.pt[1])
+                     for kp in training_target.keypoint_list]).reshape(-1, 2)
                 dtd.visualize_reprojections(
                     img_copy, kp_tmp, training_target.keypoint_list_3d,
                     camera_params.camera_int.camera_matrix,
-                    camera_params.camera_int.distortion_coeffs)
+                    camera_params.camera_int.dist_coeffs)
 
     y2020_target_list = training_target_list
     return y2020_target_list
 
+
 if __name__ == '__main__':
     ap = argparse.ArgumentParser()
-    ap.add_argument("--visualize", help="Whether to visualize the results", default=False, action='store_true')
-    ap.add_argument("--no_use_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"]
     if args["visualize"]:
-        print("Visualizing results")
+        glog.info("Visualizing results")
 
-    USE_BAZEL = args["no_use_bazel"]
-    if args["no_use_bazel"]:
-        print("Running on command line (no Bazel)")
+    USE_BAZEL = args["no_bazel"]
+    if args["no_bazel"]:
+        glog.info("Running on command line (no Bazel)")
 
     compute_target_definition()
-    pass
diff --git a/y2020/vision/tools/python_code/target_definition_test.py b/y2020/vision/tools/python_code/target_definition_test.py
new file mode 100644
index 0000000..3c4d308
--- /dev/null
+++ b/y2020/vision/tools/python_code/target_definition_test.py
@@ -0,0 +1,31 @@
+import cv2
+import numpy as np
+
+import camera_definition_test
+import define_training_data as dtd
+import target_definition
+import train_and_match as tam
+
+
+def compute_target_definition():
+    target_data_list = []
+    target_data_test_1 = target_definition.TargetData(
+        'test_images/train_power_port_red.png')
+
+    target_data_test_1.extract_features()
+
+    target_data_test_1.keypoint_list[0].pt = (0., 1.)
+
+    kp_3d = []
+    for i in range(len(target_data_test_1.keypoint_list)):
+        kp_3d.append((i, i, i))
+
+    target_data_test_1.keypoint_list_3d = np.asarray(
+        np.float32(kp_3d)).reshape(-1, 1, 3)
+
+    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/test_images/ideal_loading_bay_blue.png b/y2020/vision/tools/python_code/test_images/ideal_loading_bay_blue.png
new file mode 100644
index 0000000..c3c0aea
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/ideal_loading_bay_blue.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/test_images/ideal_loading_bay_red.png b/y2020/vision/tools/python_code/test_images/ideal_loading_bay_red.png
new file mode 100644
index 0000000..42091a6
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/ideal_loading_bay_red.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/test_images/ideal_power_port_blue.png b/y2020/vision/tools/python_code/test_images/ideal_power_port_blue.png
new file mode 100644
index 0000000..a3a7597
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/ideal_power_port_blue.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/test_images/ideal_power_port_red.png b/y2020/vision/tools/python_code/test_images/ideal_power_port_red.png
new file mode 100644
index 0000000..9d2f0bf
--- /dev/null
+++ b/y2020/vision/tools/python_code/test_images/ideal_power_port_red.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index e5a7f5b..1def399 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -1,4 +1,5 @@
 import cv2
+import glog
 import math
 import numpy as np
 import time
@@ -8,6 +9,8 @@
 FEATURE_EXTRACTOR_NAME = 'SIFT'
 QUERY_INDEX = 0  # We use a list for both training and query info, but only ever have one query item
 
+glog.setLevel("WARN")
+
 
 # Calculates rotation matrix to euler angles
 # The result is the same as MATLAB except the order
@@ -38,7 +41,7 @@
         # Load image (in color; let opencv convert to B&W for features)
         img_data = cv2.imread(im)
         if img_data is None:
-            print("Failed to load image: ", im)
+            glog.error("Failed to load image: '%s'" % im)
             exit()
         else:
             image_list.append(img_data)
@@ -143,15 +146,12 @@
 
     elif FEATURE_EXTRACTOR_NAME is 'ORB':
         matches = matcher.knnMatch(train_keypoint_lists[0], desc_query, k=2)
-        print(matches)
         good_matches = []
         for m in matches:
             if m:
                 if len(m) == 2:
-                    print(m[0].distance, m[1].distance)
                     if m[0].distance < 0.7 * m[1].distance:
                         good_matches.append(m[0])
-                        print(m[0].distance)
 
         good_matches_list.append(good_matches)
 
@@ -176,14 +176,16 @@
     for i in range(len(train_keypoint_lists)):
         good_matches = good_matches_list[i]
         if len(good_matches) < MIN_MATCH_COUNT:
-            print("Not enough matches are for model ", i, ": ",
-                  len(good_matches), " out of needed #: ", 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
 
-        print("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
@@ -206,7 +208,7 @@
 # Also shows image with query unwarped (to match training image) and target pt
 def show_results(training_images, train_keypoint_lists, query_images,
                  query_keypoint_lists, target_point_list, good_matches_list):
-    print("Showing results for ", len(training_images), " training images")
+    glog.info("Showing results for ", len(training_images), " training images")
 
     homography_list, matches_mask_list = compute_homographies(
         train_keypoint_lists, query_keypoint_lists, good_matches_list)
@@ -214,15 +216,15 @@
         good_matches = good_matches_list[i]
         if len(good_matches) < MIN_MATCH_COUNT:
             continue
-        print("Showing results for model ", i)
+        glog.debug("Showing results for model ", i)
         matches_mask_count = matches_mask_list[i].count(1)
         if matches_mask_count != len(good_matches):
-            print("Homography rejected some matches!  From ",
-                  len(good_matches), ", only ", matches_mask_count,
-                  " were used")
+            glog.info("Homography rejected some matches!  From ",
+                      len(good_matches), ", only ", matches_mask_count,
+                      " were used")
 
         if matches_mask_count < MIN_MATCH_COUNT:
-            print(
+            glog.info(
                 "Skipping match because homography rejected matches down to below ",
                 MIN_MATCH_COUNT)
             continue
@@ -235,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], 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
@@ -260,14 +263,13 @@
         img3 = cv2.drawMatches(query_image, query_keypoint_lists[QUERY_INDEX],
                                training_images[i], train_keypoint_lists[i],
                                good_matches_list[i], None, **draw_params)
-        print("Drawing matches for model ", i,
-              ".  Query on left, Training image on right")
+        glog.debug("Drawing matches for model ", i,
+                   ".  Query on left, Training image on right")
         cv2.imshow('Matches', img3), cv2.waitKey()
 
         # Next, unwarp the query image so it looks like the training view
         H_inv = np.linalg.inv(H)
         query_image_warp = cv2.warpPerspective(query_image, H_inv, (w, h))
-        print("Showing unwarped query image for model ", i)
         cv2.imshow('Unwarped Image', query_image_warp), cv2.waitKey()
 
     # Go ahead and return these, for use elsewhere
@@ -279,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/vision/tools/python_code/transform_projection_test.py b/y2020/vision/tools/python_code/transform_projection_test.py
index 00f68bb..01fe695 100644
--- a/y2020/vision/tools/python_code/transform_projection_test.py
+++ b/y2020/vision/tools/python_code/transform_projection_test.py
@@ -8,7 +8,7 @@
 # Import camera from camera_definition
 camera_params = camera_definition.web_cam_params
 cam_mat = camera_params.camera_int.camera_matrix
-distortion_coeffs = camera_params.camera_int.distortion_coeffs
+dist_coeffs = camera_params.camera_int.dist_coeffs
 
 # Height of camera on robot above ground
 cam_above_ground = 0.5
@@ -48,11 +48,11 @@
 
 pts_proj_2d_cam2, jac_2d = cv2.projectPoints(
     pts_3d_target, R_cam_cam2_gt_mat_inv, T_cam_cam2_gt_inv, cam_mat,
-    distortion_coeffs)
+    dist_coeffs)
 
 # Now, solve for the pose using the original 3d points (pts_3d_T_t) and the projections from the new location
 retval, R_cam2_cam_est, T_cam2_cam_est, inliers = cv2.solvePnPRansac(
-    pts_3d_target, pts_proj_2d_cam2, cam_mat, distortion_coeffs)
+    pts_3d_target, pts_proj_2d_cam2, cam_mat, dist_coeffs)
 
 # This is the pose from camera to original target spot.  We need to invert to get back to the pose we want
 R_cam_cam2_est_mat = cv2.Rodrigues(R_cam2_cam_est)[0].T
diff --git a/y2020/vision/tools/python_code/usb_camera_stream.py b/y2020/vision/tools/python_code/usb_camera_stream.py
deleted file mode 100644
index 5d3ae91..0000000
--- a/y2020/vision/tools/python_code/usb_camera_stream.py
+++ /dev/null
@@ -1,25 +0,0 @@
-import cv2
-# Open the device at the ID X for /dev/videoX
-cap = cv2.VideoCapture(2)
-
-#Check whether user selected camera is opened successfully.
-if not (cap.isOpened()):
-    print("Could not open video device")
-    quit()
-
-while True:
-    # Capture frame-by-frame
-    ret, frame = cap.read()
-
-    exp = cap.get(cv2.CAP_PROP_EXPOSURE)
-    print("Exposure:", exp)
-    # Display the resulting frame
-    cv2.imshow('preview', frame)
-
-    #Waits for a user input to quit the application
-    if cv2.waitKey(1) & 0xFF == ord('q'):
-        break
-
-# When everything done, release the capture
-cap.release()
-cv2.destroyAllWindows()
diff --git a/y2020/vision/v4l2_reader.cc b/y2020/vision/v4l2_reader.cc
index f1944c1..91777c7 100644
--- a/y2020/vision/v4l2_reader.cc
+++ b/y2020/vision/v4l2_reader.cc
@@ -6,6 +6,9 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 
+DEFINE_bool(ignore_timestamps, false,
+            "Don't require timestamps on images.  Used to allow webcams");
+
 namespace frc971 {
 namespace vision {
 
@@ -137,8 +140,11 @@
            buffer.m.userptr);
   CHECK_EQ(ImageSize(), buffer.length);
   CHECK(buffer.flags & V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC);
-  CHECK_EQ(buffer.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK,
-           static_cast<uint32_t>(V4L2_BUF_FLAG_TSTAMP_SRC_EOF));
+  if (!FLAGS_ignore_timestamps) {
+    // Require that we have good timestamp on images
+    CHECK_EQ(buffer.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK,
+             static_cast<uint32_t>(V4L2_BUF_FLAG_TSTAMP_SRC_EOF));
+  }
   return {static_cast<int>(buffer.index),
           aos::time::from_timeval(buffer.timestamp)};
 }
diff --git a/y2020/vision/viewer.cc b/y2020/vision/viewer.cc
new file mode 100644
index 0000000..be9b980
--- /dev/null
+++ b/y2020/vision/viewer.cc
@@ -0,0 +1,46 @@
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2020/vision/vision_generated.h"
+
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void ViewerMain() {
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  event_loop.MakeWatcher("/camera", [](const CameraImage &image) {
+    cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
+    CHECK(image_mat.isContinuous());
+    const int number_pixels = image.rows() * image.cols();
+    for (int i = 0; i < number_pixels; ++i) {
+      reinterpret_cast<uint8_t *>(image_mat.data)[i] =
+          image.data()->data()[i * 2];
+    }
+
+    cv::imshow("Display", image_mat);
+    cv::waitKey(1);
+  });
+
+  event_loop.Run();
+}
+
+}  // namespace
+}  // namespace vision
+}  // namespace frc971
+
+// Quick and lightweight grayscale viewer for images
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  frc971::vision::ViewerMain();
+}
diff --git a/y2020/vision/viewer_replay.cc b/y2020/vision/viewer_replay.cc
new file mode 100644
index 0000000..82ab11a
--- /dev/null
+++ b/y2020/vision/viewer_replay.cc
@@ -0,0 +1,65 @@
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/logging/logger.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "y2020/vision/vision_generated.h"
+
+DEFINE_string(config, "y2020/config.json", "Path to the config file to use.");
+DEFINE_string(logfile, "", "Path to the log file to use.");
+DEFINE_string(node, "pi1", "Node name to replay.");
+DEFINE_string(image_save_prefix, "/tmp/img",
+              "Prefix to use for saving images from the logfile.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void ViewerMain() {
+  CHECK(!FLAGS_logfile.empty()) << "You forgot to specify a logfile.";
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::logger::LogReader reader(FLAGS_logfile, &config.message());
+  reader.Register();
+  const aos::Node *node = nullptr;
+  if (aos::configuration::MultiNode(reader.configuration())) {
+    node = aos::configuration::GetNode(reader.configuration(), FLAGS_node);
+  }
+  std::unique_ptr<aos::EventLoop> event_loop =
+      reader.event_loop_factory()->MakeEventLoop("player", node);
+
+  int image_count = 0;
+  event_loop->MakeWatcher("/camera", [&image_count](const CameraImage &image) {
+    cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
+    CHECK(image_mat.isContinuous());
+    const int number_pixels = image.rows() * image.cols();
+    for (int i = 0; i < number_pixels; ++i) {
+      reinterpret_cast<uint8_t *>(image_mat.data)[i] =
+          image.data()->data()[i * 2];
+    }
+
+    cv::imshow("Display", image_mat);
+    if (!FLAGS_image_save_prefix.empty()) {
+      cv::imwrite("/tmp/img" + std::to_string(image_count++) + ".png",
+                  image_mat);
+    }
+    cv::waitKey(1);
+  });
+
+  reader.event_loop_factory()->Run();
+}
+
+}  // namespace
+}  // namespace vision
+}  // namespace frc971
+
+// Quick and lightweight grayscale viewer for images
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  frc971::vision::ViewerMain();
+}
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 91182fc..4f39acf 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -218,8 +218,7 @@
 
     {
       auto builder = superstructure_position_sender_.MakeBuilder();
-      superstructure::Position::Builder position_builder =
-          builder.MakeBuilder<superstructure::Position>();
+
       // TODO(alex): check new absolute encoder api.
       // Hood
       frc971::AbsolutePositionT hood;
@@ -246,6 +245,14 @@
       flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
           frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &turret);
 
+      // Control Panel
+      frc971::RelativePositionT control_panel;
+      CopyPosition(*control_panel_encoder_, &control_panel,
+                   Values::kControlPanelEncoderCountsPerRevolution(),
+                   Values::kControlPanelEncoderRatio(), false);
+      flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
+          frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
+
       // Shooter
       y2020::control_loops::superstructure::ShooterPositionT shooter;
       shooter.theta_finisher =
@@ -266,14 +273,8 @@
               y2020::control_loops::superstructure::ShooterPosition::Pack(
                   *builder.fbb(), &shooter);
 
-      // Control Panel
-      frc971::RelativePositionT control_panel;
-      CopyPosition(*control_panel_encoder_, &control_panel,
-                   Values::kControlPanelEncoderCountsPerRevolution(),
-                   Values::kControlPanelEncoderRatio(), false);
-      flatbuffers::Offset<frc971::RelativePosition> control_panel_offset =
-          frc971::RelativePosition::Pack(*builder.fbb(), &control_panel);
-
+      superstructure::Position::Builder position_builder =
+          builder.MakeBuilder<superstructure::Position>();
       position_builder.add_hood(hood_offset);
       position_builder.add_intake_joint(intake_joint_offset);
       position_builder.add_turret(turret_offset);
@@ -427,7 +428,7 @@
     if (climber_falcon_) {
       climber_falcon_->Set(
           ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
-          std::clamp(output.climber_voltage(), -kMaxBringupPower,
+          std::clamp(-output.climber_voltage(), -kMaxBringupPower,
                      kMaxBringupPower) /
               12.0);
     }
@@ -481,6 +482,7 @@
     // Thread 3.
     ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
     SensorReader sensor_reader(&sensor_reader_event_loop);
+    sensor_reader.set_pwm_trigger(true);
     sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
     sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
     // TODO: pin numbers
@@ -550,8 +552,8 @@
         make_unique<::frc::TalonFX>(4));
     superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(9));
     // TODO: check port
-    //superstructure_writer.set_climber_falcon(
-        //make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(11));
+    superstructure_writer.set_climber_falcon(
+        make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
 
     AddLoop(&output_event_loop);
 
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index c0daa07..6b27c66 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -1,25 +1,48 @@
 load("@build_bazel_rules_typescript//:defs.bzl", "ts_library")
 load("@build_bazel_rules_nodejs//:defs.bzl", "rollup_bundle")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
 
 ts_library(
-    name = "main",
+    name = "camera_main",
     srcs = [
-        "main.ts",
         "image_handler.ts",
+        "camera_main.ts",
     ],
+    visibility = ["//y2020:__subpackages__"],
     deps = [
         "//aos/network/www:proxy",
         "//y2020/vision:vision_ts_fbs",
         "//y2020/vision/sift:sift_ts_fbs",
     ],
-    visibility = ["//y2020:__subpackages__"],
+)
+
+ts_library(
+    name = "field_main",
+    srcs = [
+        "field_main.ts",
+        "field_handler.ts",
+        "constants.ts",
+    ],
+    deps = [
+        "//aos/network/www:proxy",
+        "//y2020/vision/sift:sift_ts_fbs",
+    ],
 )
 
 rollup_bundle(
-    name = "main_bundle",
-    entry_point = "y2020/www/main",
+    name = "camera_main_bundle",
+    entry_point = "y2020/www/camera_main",
+    visibility = ["//y2020:__subpackages__"],
     deps = [
-        "main",
+        "camera_main",
+    ],
+)
+
+rollup_bundle(
+    name = "field_main_bundle",
+    entry_point = "y2020/www/field_main",
+    deps = [
+        "field_main",
     ],
     visibility = ["//y2020:__subpackages__"],
 )
@@ -30,7 +53,7 @@
         "**/*.html",
         "**/*.css",
     ]),
-    visibility=["//visibility:public"],
+    visibility = ["//visibility:public"],
 )
 
 genrule(
@@ -42,5 +65,17 @@
         "flatbuffers.js",
     ],
     cmd = "cp $(location @com_github_google_flatbuffers//:flatjs) $@",
-    visibility=["//y2020:__subpackages__"],
+    visibility = ["//y2020:__subpackages__"],
+)
+
+aos_downloader_dir(
+    name = "www_files",
+    srcs = [
+        ":camera_main_bundle",
+        ":field_main_bundle",
+        ":files",
+        ":flatbuffers",
+    ],
+    dir = "www",
+    visibility = ["//visibility:public"],
 )
diff --git a/y2020/www/camera_main.ts b/y2020/www/camera_main.ts
new file mode 100644
index 0000000..e13452e
--- /dev/null
+++ b/y2020/www/camera_main.ts
@@ -0,0 +1,10 @@
+import {Connection} from 'aos/network/www/proxy';
+
+import {ImageHandler} from './image_handler';
+import {ConfigHandler} from 'aos/network/www/config_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const iHandler = new ImageHandler(conn);
diff --git a/y2020/www/camera_viewer.html b/y2020/www/camera_viewer.html
new file mode 100644
index 0000000..4a61d7a
--- /dev/null
+++ b/y2020/www/camera_viewer.html
@@ -0,0 +1,10 @@
+<html>
+  <head>
+    <script src="flatbuffers.js"></script>
+    <script src="camera_main_bundle.min.js" defer></script>
+    <link rel="stylesheet" href="styles.css">
+  </head>
+  <body>
+  </body>
+</html>
+
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..ad449db
--- /dev/null
+++ b/y2020/www/field.html
@@ -0,0 +1,10 @@
+<html>
+  <head>
+    <script src="flatbuffers.js"></script>
+    <script src="field_main_bundle.min.js" defer></script>
+    <link rel="stylesheet" href="styles.css">
+  </head>
+  <body>
+  </body>
+</html>
+
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
new file mode 100644
index 0000000..0d2ea34
--- /dev/null
+++ b/y2020/www/field_handler.ts
@@ -0,0 +1,241 @@
+import {Configuration, Channel} from 'aos/configuration_generated';
+import {Connection} from 'aos/network/www/proxy';
+import {Connect} from 'aos/network/connect_generated';
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+import {ImageMatchResult} from 'y2020/vision/sift/sift_generated'
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const DS_WIDTH = 69 * IN_TO_M;
+const DS_ANGLE = 20 * Math.PI / 180;
+const DS_END_X = FIELD_EDGE_X - DS_WIDTH * Math.sin(DS_ANGLE);
+const DS_INSIDE_Y = FIELD_SIDE_Y - DS_WIDTH * Math.cos(DS_ANGLE);
+
+const TRENCH_X = 108 * IN_TO_M;
+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 - FIELD_EDGE_X;
+const SPINNER_BOTTOM_X = SPINNER_TOP_X - SPINNER_LENGTH;
+
+const SHIELD_BOTTOM_X = -116 * IN_TO_M;
+const SHIELD_BOTTOM_Y = 43.75 * IN_TO_M;
+
+const SHIELD_TOP_X = 116 * IN_TO_M;
+const SHIELD_TOP_Y = -43.75 * IN_TO_M;
+
+const SHIELD_RIGHT_X = -51.06 * IN_TO_M;
+const SHIELD_RIGHT_Y = -112.88 * IN_TO_M;
+
+const SHIELD_LEFT_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 = FIELD_EDGE_X - 120 * IN_TO_M;
+
+const TARGET_ZONE_TIP_X = FIELD_EDGE_X - 30 * IN_TO_M;
+const TARGET_ZONE_WIDTH = 48 * IN_TO_M;
+const LOADING_ZONE_WIDTH = 60 * IN_TO_M;
+
+/**
+ * All the messages that are required to display camera information on the field.
+ * Messages not readable on the server node are ignored.
+ */
+const REQUIRED_CHANNELS = [
+  {
+    name: '/pi1/camera',
+    type: 'frc971.vision.sift.ImageMatchResult',
+  },
+  {
+    name: '/pi2/camera',
+    type: 'frc971.vision.sift.ImageMatchResult',
+  },
+  {
+    name: '/pi3/camera',
+    type: 'frc971.vision.sift.ImageMatchResult',
+  },
+  {
+    name: '/pi4/camera',
+    type: 'frc971.vision.sift.ImageMatchResult',
+  },
+  {
+    name: '/pi5/camera',
+    type: 'frc971.vision.sift.ImageMatchResult',
+  },
+];
+
+export class FieldHandler {
+  private canvas = document.createElement('canvas');
+  private imageMatchResult :ImageMatchResult|null = null
+
+  constructor(private readonly connection: Connection) {
+    document.body.appendChild(this.canvas);
+
+    this.connection.addConfigHandler(() => {
+      this.sendConnect();
+    });
+    this.connection.addHandler(ImageMatchResult.getFullyQualifiedName(), (res) => {
+      this.handleImageMatchResult(res);
+    });
+  }
+
+  private handleImageMatchResult(data: Uint8Array): void {
+    const fbBuffer = new flatbuffers.ByteBuffer(data);
+    this.imageMatchResult = ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
+  }
+
+  private sendConnect(): void {
+    const builder = new flatbuffers.Builder(512);
+    const channels: flatbuffers.Offset[] = [];
+    for (const channel of REQUIRED_CHANNELS) {
+      const nameFb = builder.createString(channel.name);
+      const typeFb = builder.createString(channel.type);
+      Channel.startChannel(builder);
+      Channel.addName(builder, nameFb);
+      Channel.addType(builder, typeFb);
+      const channelFb = Channel.endChannel(builder);
+      channels.push(channelFb);
+    }
+
+    const channelsFb = Connect.createChannelsToTransferVector(builder, channels);
+    Connect.startConnect(builder);
+    Connect.addChannelsToTransfer(builder, channelsFb);
+    const connect = Connect.endConnect(builder);
+    builder.finish(connect);
+    this.connection.sendConnectMessage(builder);
+  }
+
+  drawField(): void {
+    const MY_COLOR = 'red';
+    const OTHER_COLOR = 'blue';
+    const ctx = this.canvas.getContext('2d');
+    // draw perimiter
+    ctx.beginPath();
+    ctx.moveTo(FIELD_EDGE_X, DS_INSIDE_Y);
+    ctx.lineTo(DS_END_X, FIELD_SIDE_Y);
+    ctx.lineTo(-DS_END_X, FIELD_SIDE_Y);
+    ctx.lineTo(-FIELD_EDGE_X, DS_INSIDE_Y);
+    ctx.lineTo(-FIELD_EDGE_X, -DS_INSIDE_Y);
+    ctx.lineTo(-DS_END_X, -FIELD_SIDE_Y);
+    ctx.lineTo(DS_END_X, -FIELD_SIDE_Y);
+    ctx.lineTo(FIELD_EDGE_X, -DS_INSIDE_Y);
+    ctx.lineTo(FIELD_EDGE_X, 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();
+
+    this.drawHalfField(ctx, 'red');
+    ctx.rotate(Math.PI);
+    this.drawHalfField(ctx, 'blue');
+    ctx.rotate(Math.PI);
+  }
+
+  drawHalfField(ctx, color: string): void {
+    // trenches
+    ctx.strokeStyle = color;
+    ctx.beginPath();
+    ctx.moveTo(TRENCH_X, FIELD_SIDE_Y);
+    ctx.lineTo(TRENCH_X, TRENCH_INSIDE);
+    ctx.lineTo(-TRENCH_X, TRENCH_INSIDE);
+    ctx.lineTo(-TRENCH_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.stroke();
+
+    ctx.beginPath();
+    ctx.moveTo(INITIATION_X, FIELD_SIDE_Y);
+    ctx.lineTo(INITIATION_X, -FIELD_SIDE_Y);
+    ctx.stroke();
+
+    // target/loading
+    ctx.strokeStyle = color;
+    ctx.beginPath();
+    ctx.moveTo(FIELD_EDGE_X, DS_INSIDE_Y);
+    ctx.lineTo(TARGET_ZONE_TIP_X, DS_INSIDE_Y - 0.5 * TARGET_ZONE_WIDTH);
+    ctx.lineTo(FIELD_EDGE_X, DS_INSIDE_Y - TARGET_ZONE_WIDTH);
+
+    ctx.moveTo(-FIELD_EDGE_X, DS_INSIDE_Y);
+    ctx.lineTo(-TARGET_ZONE_TIP_X, DS_INSIDE_Y - 0.5 * LOADING_ZONE_WIDTH);
+    ctx.lineTo(-FIELD_EDGE_X, DS_INSIDE_Y - LOADING_ZONE_WIDTH);
+    ctx.stroke();
+  }
+
+  drawCamera(x: number, y: number, theta: number): void {
+    const ctx = this.canvas.getContext('2d');
+    ctx.save();
+    ctx.translate(x, y);
+    ctx.rotate(theta);
+    ctx.beginPath();
+    ctx.moveTo(0.5, 0.5);
+    ctx.lineTo(0, 0);
+    ctx.lineTo(0.5, -0.5);
+    ctx.stroke();
+    ctx.beginPath();
+    ctx.arc(0, 0, 0.25, -Math.PI/4, Math.PI/4);
+    ctx.stroke();
+    ctx.restore();
+  }
+
+  draw(): void  {
+    this.reset();
+    this.drawField();
+    //draw cameras
+    if (this.imageMatchResult) {
+      for (const i = 0; i < this.imageMatchResult.cameraPosesLength(); i++) {
+        const pose = this.imageMatchResult.cameraPoses(i);
+        const mat = pose.fieldToCamera();
+        const x = mat.data(3);
+        const y = mat.data(7);
+        this.drawCamera(x, y, 0);
+        console.log(x, y);
+      }
+    }
+
+    window.requestAnimationFrame(() => this.draw());
+  }
+
+  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;
+    const width = size / 2 + 20;
+    ctx.canvas.width = width;
+    ctx.clearRect(0, 0, size, width);
+
+    // Translate to center of display.
+    ctx.translate(width / 2, size / 2);
+    // Coordinate system is:
+    // x -> forward.
+    // y -> to the left.
+    ctx.rotate(-Math.PI / 2);
+    ctx.scale(1, -1);
+
+    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..163c817
--- /dev/null
+++ b/y2020/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from 'aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+fieldHandler.draw();
+
diff --git a/y2020/www/image_handler.ts b/y2020/www/image_handler.ts
index 802e448..a44156a 100644
--- a/y2020/www/image_handler.ts
+++ b/y2020/www/image_handler.ts
@@ -1,36 +1,134 @@
+import {Channel} from 'aos/configuration_generated';
+import {Connection} from 'aos/network/www/proxy';
+import {Connect} from 'aos/network/connect_generated';
+import {ImageMatchResult} from 'y2020/vision/sift/sift_generated'
 import {CameraImage} from 'y2020/vision/vision_generated';
 
+/*
+ * All the messages that are required to show an image with metadata.
+ * Messages not readable on the server node are ignored.
+ */
+const REQUIRED_CHANNELS = [
+  {
+    name: '/pi1/camera',
+    type: CameraImage.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi2/camera',
+    type: CameraImage.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi3/camera',
+    type: CameraImage.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi4/camera',
+    type: CameraImage.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi5/camera',
+    type: CameraImage.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi1/camera/detailed',
+    type: ImageMatchResult.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi2/camera/detailed',
+    type: ImageMatchResult.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi3/camera/detailed',
+    type: ImageMatchResult.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi4/camera/detailed',
+    type: ImageMatchResult.getFullyQualifiedName(),
+  },
+  {
+    name: '/pi5/camera/detailed',
+    type: ImageMatchResult.getFullyQualifiedName(),
+  },
+];
+
 export class ImageHandler {
   private canvas = document.createElement('canvas');
   private imageBuffer: Uint8ClampedArray|null = null;
+  private image: CameraImage|null = null;
   private imageTimestamp: flatbuffers.Long|null = null;
-  private result: fr971.vision.ImageMatchResult|null = null;
+  private result: ImageMatchResult|null = null;
   private resultTimestamp: flatbuffers.Long|null = null;
+  private width = 0;
+  private height = 0;
+  private imageSkipCount = 3;
 
-  constructor() {
+  constructor(private readonly connection: Connection) {
     document.body.appendChild(this.canvas);
+
+    this.connection.addConfigHandler(() => {
+      this.sendConnect();
+    });
+    this.connection.addHandler(ImageMatchResult.getFullyQualifiedName(), (data) => {
+      this.handleImageMetadata(data);
+    });
+    this.connection.addHandler(CameraImage.getFullyQualifiedName(), (data) => {
+      this.handleImage(data);
+    });
+  }
+
+  private sendConnect(): void {
+    const builder = new flatbuffers.Builder(512);
+    const channels: flatbuffers.Offset[] = [];
+    for (const channel of REQUIRED_CHANNELS) {
+      const nameFb = builder.createString(channel.name);
+      const typeFb = builder.createString(channel.type);
+      Channel.startChannel(builder);
+      Channel.addName(builder, nameFb);
+      Channel.addType(builder, typeFb);
+      const channelFb = Channel.endChannel(builder);
+      channels.push(channelFb);
+    }
+
+    const channelsFb = Connect.createChannelsToTransferVector(builder, channels);
+    Connect.startConnect(builder);
+    Connect.addChannelsToTransfer(builder, channelsFb);
+    const connect = Connect.endConnect(builder);
+    builder.finish(connect);
+    this.connection.sendConnectMessage(builder);
   }
 
   handleImage(data: Uint8Array): void {
-    const fbBuffer = new flatbuffers.ByteBuffer(data);
-    const image = CameraImage.getRootAsCameraImage(fbBuffer);
-    this.imageTimestamp = image.monotonicTimestampNs();
+    console.log('got an image to process');
+    if (this.imageSkipCount != 0) {
+      this.imageSkipCount--;
+      return;
+    } else {
+      this.imageSkipCount = 3;
+    }
 
-    const width = image.cols();
-    const height = image.rows();
-    if (width === 0 || height === 0) {
+    const fbBuffer = new flatbuffers.ByteBuffer(data);
+    this.image = CameraImage.getRootAsCameraImage(fbBuffer);
+    this.imageTimestamp = this.image.monotonicTimestampNs();
+
+    this.width = this.image.cols();
+    this.height = this.image.rows();
+    if (this.width === 0 || this.height === 0) {
       return;
     }
-    this.imageBuffer = new Uint8ClampedArray(width * height * 4); // RGBA
 
+    this.draw();
+  }
+
+  convertImage(): void {
+    this.imageBuffer = new Uint8ClampedArray(this.width * this.height * 4); // RGBA
     // Read four bytes (YUYV) from the data and transform into two pixels of
     // RGBA for canvas
-    for (const j = 0; j < height; j++) {
-      for (const i = 0; i < width; i += 2) {
-        const y1 = image.data((j * width + i) * 2);
-        const u = image.data((j * width + i) * 2 + 1);
-        const y2 = image.data((j * width + i + 1) * 2);
-        const v = image.data((j * width + i + 1) * 2 + 1);
+    for (const j = 0; j < this.height; j++) {
+      for (const i = 0; i < this.width; i += 2) {
+        const y1 = this.image.data((j * this.width + i) * 2);
+        const u = this.image.data((j * this.width + i) * 2 + 1);
+        const y2 = this.image.data((j * this.width + i + 1) * 2);
+        const v = this.image.data((j * this.width + i + 1) * 2 + 1);
 
         // Based on https://en.wikipedia.org/wiki/YUV#Converting_between_Y%E2%80%B2UV_and_RGB
         const c1 = y1 - 16;
@@ -38,60 +136,57 @@
         const d = u - 128;
         const e = v - 128;
 
-        imageBuffer[(j * width + i) * 4 + 0] = (298 * c1 + 409 * e + 128) >> 8;
-        imageBuffer[(j * width + i) * 4 + 1] =
-            (298 * c1 - 100 * d - 208 * e + 128) >> 8;
-        imageBuffer[(j * width + i) * 4 + 2] = (298 * c1 + 516 * d + 128) >> 8;
-        imageBuffer[(j * width + i) * 4 + 3] = 255;
-        imageBuffer[(j * width + i) * 4 + 4] = (298 * c2 + 409 * e + 128) >> 8;
-        imageBuffer[(j * width + i) * 4 + 5] =
-            (298 * c2 - 100 * d - 208 * e + 128) >> 8;
-        imageBuffer[(j * width + i) * 4 + 6] = (298 * c2 + 516 * d + 128) >> 8;
-        imageBuffer[(j * width + i) * 4 + 7] = 255;
+        this.imageBuffer[(j * this.width + i) * 4 + 0] = (298 * c1 + 409 * e + 128) >> 8;
+        this.imageBuffer[(j * this.width + i) * 4 + 1] = (298 * c1 - 100 * d - 208 * e + 128) >> 8;
+        this.imageBuffer[(j * this.width + i) * 4 + 2] = (298 * c1 + 516 * d + 128) >> 8;
+        this.imageBuffer[(j * this.width + i) * 4 + 3] = 255;
+        this.imageBuffer[(j * this.width + i) * 4 + 4] = (298 * c2 + 409 * e + 128) >> 8;
+        this.imageBuffer[(j * this.width + i) * 4 + 5] = (298 * c2 - 100 * d - 208 * e + 128) >> 8;
+        this.imageBuffer[(j * this.width + i) * 4 + 6] = (298 * c2 + 516 * d + 128) >> 8;
+        this.imageBuffer[(j * this.width + i) * 4 + 7] = 255;
       }
     }
-
-    draw();
   }
 
   handleImageMetadata(data: Uint8Array): void {
+    console.log('got an image match result to process');
     const fbBuffer = new flatbuffers.ByteBuffer(data);
-    this.result = frc971.vision.ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
-    this.resultTimestamp = result.imageMonotonicTimestampNs();
-    draw();
+    this.result = ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
+    this.resultTimestamp = this.result.imageMonotonicTimestampNs();
+    this.draw();
   }
 
   draw(): void {
-    if (imageTimestamp.low !== resultTimestamp.low ||
-        imageTimestamp.high !== resultTimestamp.high) {
+  if (!this.imageTimestamp || !this.resultTimestamp ||
+        this.imageTimestamp.low !== this.resultTimestamp.low ||
+        this.imageTimestamp.high !== this.resultTimestamp.high) {
+      console.log('image and result do not match');
+      console.log(this.imageTimestamp.low, this.resultTimestamp.low);
+      console.log(this.imageTimestamp.high, this.resultTimestamp.high);
       return;
     }
+    this.convertImage();
     const ctx = this.canvas.getContext('2d');
 
-    this.canvas.width = width;
-    this.canvas.height = height;
-    const idata = ctx.createImageData(width, height);
+    this.canvas.width = this.width;
+    this.canvas.height = this.height;
+    const idata = ctx.createImageData(this.width, this.height);
     idata.data.set(this.imageBuffer);
     ctx.putImageData(idata, 0, 0);
-    ctx.beginPath();
-    for (const feature of this.result.getFeatures()) {
+    for (const i = 0; i < this.result.featuresLength(); i++) {
+      const feature = this.result.features(i);
       // Based on OpenCV drawKeypoint.
-      ctx.arc(feature.x, feature.y, feature.size, 0, 2 * Math.PI);
-      ctx.moveTo(feature.x, feature.y);
-      // TODO(alex): check that angle is correct (0?, direction?)
-      const angle = feature.angle * Math.PI / 180;
+      ctx.beginPath();
+      ctx.arc(feature.x(), feature.y(), feature.size(), 0, 2 * Math.PI);
+      ctx.stroke();
+
+      ctx.beginPath();
+      ctx.moveTo(feature.x(), feature.y());
+      const angle = feature.angle() * Math.PI / 180;
       ctx.lineTo(
-          feature.x + feature.radius * cos(angle),
-          feature.y + feature.radius * sin(angle));
+          feature.x() + feature.size() * Math.cos(angle),
+          feature.y() + feature.size() * Math.sin(angle));
+      ctx.stroke();
     }
-    ctx.stroke();
-  }
-
-  getId(): string {
-    return CameraImage.getFullyQualifiedName();
-  }
-
-  getResultId(): string {
-    return frc971.vision.ImageMatchResult.getFullyQualifiedName();
   }
 }
diff --git a/y2020/www/index.html b/y2020/www/index.html
index 97e16d4..f9ceca2 100644
--- a/y2020/www/index.html
+++ b/y2020/www/index.html
@@ -1,11 +1,6 @@
 <html>
-  <head>
-    <script src="flatbuffers.js"></script>
-    <script src="main_bundle.min.js" defer></script>
-    <link rel="stylesheet" href="styles.css">
-  </head>
   <body>
-    <div id="config">
-    </div>
+    <a href="camera_viewer.html">Camera Viewer</a><br>
+    <a href="field.html">Field Visualization</a>
   </body>
 </html>
diff --git a/y2020/www/main.ts b/y2020/www/main.ts
deleted file mode 100644
index d414eac..0000000
--- a/y2020/www/main.ts
+++ /dev/null
@@ -1,13 +0,0 @@
-import {Connection} from 'aos/network/www/proxy';
-
-import {ImageHandler} from './image_handler';
-
-const conn = new Connection();
-
-conn.connect();
-
-const iHandler = new ImageHandler();
-
-conn.addHandler(iHandler.getId(), (data) => iHandler.handleImage(data));
-conn.addHandler(
-    iHandler.getResultId(), (data) => iHandler.handleImageMetadata(data));
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 3673eee..b40ac87 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -60,7 +60,7 @@
       "name": "/aos/roborio",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "roborio",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -114,7 +114,7 @@
       "name": "/aos/pi1",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "pi1",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -158,7 +158,7 @@
       "name": "/aos/pi2",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "pi2",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -202,7 +202,7 @@
       "name": "/aos/pi3",
       "type": "aos.message_bridge.ClientStatistics",
       "source_node": "pi3",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -248,6 +248,12 @@
       "num_senders": 2
     },
     {
+      "name": "/superstructure",
+      "type": "y2020.joysticks.Setpoint",
+      "source_node": "roborio",
+      "num_senders": 2
+    },
+    {
       "name": "/drivetrain",
       "type": "frc971.IMUValues",
       "source_node": "roborio",
@@ -320,6 +326,42 @@
       "type": "frc971.vision.sift.ImageMatchResult",
       "source_node": "pi1",
       "frequency": 25,
+      "max_size": 10000,
+      "destination_nodes": [
+        {
+          "name": "roborio",
+          "priority": 1,
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/pi1/camera/detailed",
+      "type": "frc971.vision.sift.ImageMatchResult",
+      "source_node": "pi1",
+      "frequency": 25,
+      "max_size": 1000000
+    },
+    {
+      "name": "/pi1/camera",
+      "type": "frc971.vision.sift.TrainingData",
+      "source_node": "pi1",
+      "frequency": 2,
+      "max_size": 2000000
+    },
+    {
+      "name": "/pi2/camera",
+      "type": "frc971.vision.CameraImage",
+      "source_node": "pi2",
+      "frequency": 25,
+      "max_size": 620000,
+      "num_senders": 18
+    },
+    {
+      "name": "/pi2/camera",
+      "type": "frc971.vision.sift.ImageMatchResult",
+      "source_node": "pi2",
+      "frequency": 25,
       "max_size": 300000,
       "destination_nodes": [
         {
@@ -330,6 +372,56 @@
       ]
     },
     {
+      "name": "/pi2/camera/detailed",
+      "type": "frc971.vision.sift.ImageMatchResult",
+      "source_node": "pi2",
+      "frequency": 25,
+      "max_size": 1000000
+    },
+    {
+      "name": "/pi2/camera",
+      "type": "frc971.vision.sift.TrainingData",
+      "source_node": "pi2",
+      "frequency": 2,
+      "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": 1000000
+    },
+    {
+      "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"
@@ -387,6 +479,24 @@
     },
     {
       "match": {
+        "name": "/camera",
+        "source_node": "pi1"
+      },
+      "rename": {
+        "name": "/pi1/camera"
+      }
+    },
+    {
+      "match": {
+        "name": "/camera/detailed",
+        "source_node": "pi1"
+      },
+      "rename": {
+        "name": "/pi1/camera/detailed"
+      }
+    },
+    {
+      "match": {
         "name": "/aos",
         "source_node": "pi2"
       },
@@ -396,6 +506,24 @@
     },
     {
       "match": {
+        "name": "/camera",
+        "source_node": "pi2"
+      },
+      "rename": {
+        "name": "/pi2/camera"
+      }
+    },
+    {
+      "match": {
+        "name": "/camera/detailed",
+        "source_node": "pi2"
+      },
+      "rename": {
+        "name": "/pi2/camera/detailed"
+      }
+    },
+    {
+      "match": {
         "name": "/aos",
         "source_node": "pi3"
       },
@@ -405,6 +533,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"
       },