diff --git a/WORKSPACE b/WORKSPACE
index e7de0e2..285b1f0 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -723,11 +723,17 @@
     urls = ["https://www.frc971.org/Build-Dependencies/small_sample_logfile.fbs"],
 )
 
-http_file(
+http_archive(
     name = "drivetrain_replay",
-    downloaded_file_path = "spinning_wheels_while_still.bfbs",
-    sha256 = "8abe3bbf7ac7a3ab37ad8a313ec22fc244899d916f5e9037100b02e242f5fb45",
-    urls = ["https://www.frc971.org/Build-Dependencies/spinning_wheels_while_still4.bfbs"],
+    build_file_content = """
+filegroup(
+    name = "drivetrain_replay",
+    srcs = glob(["**/*.bfbs"]),
+    visibility = ["//visibility:public"],
+)
+    """,
+    sha256 = "115dcd2fe005cb9cad3325707aa7f4466390c43a08555edf331c06c108bdf692",
+    url = "https://www.frc971.org/Build-Dependencies/2021-03-20_drivetrain_spin_wheels.tar.gz",
 )
 
 # OpenCV armhf (for raspberry pi)
diff --git a/aos/BUILD b/aos/BUILD
index 0159623..04293df 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -335,6 +335,7 @@
         ":flatbuffer_utils",
         ":flatbuffers",
         ":json_tokenizer",
+        "//aos/util:file",
         "@com_github_google_flatbuffers//:flatbuffers",
         "@com_github_google_glog//:glog",
         "@com_google_absl//absl/strings",
diff --git a/aos/aos_send.cc b/aos/aos_send.cc
index d0dea01..d94418a 100644
--- a/aos/aos_send.cc
+++ b/aos/aos_send.cc
@@ -39,6 +39,7 @@
       cli_info.event_loop->MakeRawSender(channel);
   flatbuffers::FlatBufferBuilder fbb(sender->fbb_allocator()->size(),
                                      sender->fbb_allocator());
+  fbb.ForceDefaults(true);
   fbb.Finish(aos::JsonToFlatbuffer(std::string_view(argv[1]), channel->schema(),
                                    &fbb));
   sender->Send(fbb.GetSize());
diff --git a/aos/events/logging/log_writer.cc b/aos/events/logging/log_writer.cc
index 0b08c94..f68bbf6 100644
--- a/aos/events/logging/log_writer.cc
+++ b/aos/events/logging/log_writer.cc
@@ -249,6 +249,10 @@
   // Note: this ship may have already sailed, but we don't have to make it
   // worse.
   // TODO(austin): Test...
+  //
+  // This is safe to call here since we have set last_synchronized_time_ as the
+  // same time as in the header, and all the data before it should be logged
+  // without ordering concerns.
   LogUntil(last_synchronized_time_);
 
   timer_handler_->Setup(event_loop_->monotonic_now() + polling_period_,
@@ -260,7 +264,7 @@
   CHECK(log_namer_) << ": Not logging right now";
 
   if (end_time != aos::monotonic_clock::min_time) {
-    LogUntil(end_time);
+    DoLogData(end_time);
   }
   timer_handler_->Disable();
 
diff --git a/aos/events/logging/log_writer.h b/aos/events/logging/log_writer.h
index f5b55a7..992633a 100644
--- a/aos/events/logging/log_writer.h
+++ b/aos/events/logging/log_writer.h
@@ -247,7 +247,9 @@
 
   void WriteMissingTimestamps();
 
-  // Fetches from each channel until all the data is logged.
+  // Fetches from each channel until all the data is logged.  This is dangerous
+  // because it lets you log for more than 1 period.  All calls need to verify
+  // that t isn't greater than 1 period in the future.
   void LogUntil(monotonic_clock::time_point t);
 
   void RecordFetchResult(aos::monotonic_clock::time_point start,
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 892dd2e..80d909b 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -80,6 +80,12 @@
   closedir(directory);
 }
 
+std::vector<std::string> FindLogs(std::string filename) {
+  std::vector<std::string> files;
+  FindLogs(&files, filename);
+  return files;
+}
+
 std::vector<std::string> FindLogs(int argc, char **argv) {
   std::vector<std::string> found_logfiles;
 
diff --git a/aos/events/logging/logfile_sorting.h b/aos/events/logging/logfile_sorting.h
index 8dee3da..964e592 100644
--- a/aos/events/logging/logfile_sorting.h
+++ b/aos/events/logging/logfile_sorting.h
@@ -96,6 +96,10 @@
 // them to the vector.
 void FindLogs(std::vector<std::string> *files, std::string filename);
 
+// Recursively searches the file/folder for .bfbs and .bfbs.xz files and returns
+// them in a vector.
+std::vector<std::string> FindLogs(std::string filename);
+
 // Recursively searches for logfiles in argv[1] and onward.
 std::vector<std::string> FindLogs(int argc, char **argv);
 
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 17078dc..d8ab66e 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -28,7 +28,7 @@
 using aos::message_bridge::RemoteMessage;
 using aos::testing::MessageCounter;
 
-constexpr std::string_view kSingleConfigSha1(
+constexpr std::string_view kSingleConfigSha256(
     "bc8c9c2e31589eae6f0e36d766f6a437643e861d9568b7483106841cf7504dea");
 
 std::vector<std::vector<std::string>> ToLogReaderVector(
@@ -78,7 +78,7 @@
   const ::std::string tmpdir = aos::testing::TestTmpDir();
   const ::std::string base_name = tmpdir + "/logfile";
   const ::std::string config =
-      absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile = base_name + ".part0.bfbs";
   // Remove it.
   unlink(config.c_str());
@@ -142,11 +142,11 @@
   const ::std::string tmpdir = aos::testing::TestTmpDir();
   const ::std::string base_name1 = tmpdir + "/logfile1";
   const ::std::string config1 =
-      absl::StrCat(base_name1, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name1, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile1 = base_name1 + ".part0.bfbs";
   const ::std::string base_name2 = tmpdir + "/logfile2";
   const ::std::string config2 =
-      absl::StrCat(base_name2, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name2, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile2 = base_name2 + ".part0.bfbs";
   unlink(logfile1.c_str());
   unlink(config1.c_str());
@@ -180,7 +180,7 @@
   const ::std::string tmpdir = aos::testing::TestTmpDir();
   const ::std::string base_name = tmpdir + "/logfile";
   const ::std::string config =
-      absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile = base_name + ".part0.bfbs";
   // Remove it.
   unlink(config.c_str());
@@ -213,11 +213,11 @@
   const ::std::string tmpdir = aos::testing::TestTmpDir();
   const ::std::string base_name1 = tmpdir + "/logfile1";
   const ::std::string config1 =
-      absl::StrCat(base_name1, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name1, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile1 = base_name1 + ".part0.bfbs";
   const ::std::string base_name2 = tmpdir + "/logfile2";
   const ::std::string config2 =
-      absl::StrCat(base_name2, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name2, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile2 = base_name2 + ".part0.bfbs";
   unlink(logfile1.c_str());
   unlink(config1.c_str());
@@ -283,7 +283,7 @@
   const ::std::string tmpdir = aos::testing::TestTmpDir();
   const ::std::string base_name = tmpdir + "/logfile";
   const ::std::string config =
-      absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile0 = base_name + ".part0.bfbs";
   const ::std::string logfile1 = base_name + ".part1.bfbs";
   // Remove it.
@@ -368,7 +368,7 @@
   const ::std::string tmpdir = aos::testing::TestTmpDir();
   const ::std::string base_name = tmpdir + "/logfile";
   const ::std::string config =
-      absl::StrCat(base_name, kSingleConfigSha1, ".bfbs");
+      absl::StrCat(base_name, kSingleConfigSha256, ".bfbs");
   const ::std::string logfile = base_name + ".part0.bfbs";
   // Remove the log file.
   unlink(config.c_str());
diff --git a/aos/events/logging/timestamp_plot.gnuplot b/aos/events/logging/timestamp_plot.gnuplot
index 7ad62da..94ef7e4 100755
--- a/aos/events/logging/timestamp_plot.gnuplot
+++ b/aos/events/logging/timestamp_plot.gnuplot
@@ -23,6 +23,10 @@
 offsetfile = "/tmp/timestamp_noncausal_offsets.csv"
 
 #set term qt 0
+if (ARG3 ne "" ) {
+     set term png
+     set output ARG3
+}
 
 plot samplefile12 using 1:2 title 'sample 1-2', \
      samplefile21 using 1:(-$2) title 'sample 2-1', \
@@ -30,4 +34,8 @@
      noncausalfile21 using 1:(-$3) title 'nc 2-1' with lines, \
      offsetfile using ((column(node1_index) - node1_start_time + (column(node2_index) - node2_start_time)) / 2):(column(node2_index) - column(node1_index)) title 'filter 2-1' with linespoints
 
+if (ARG3 ne "" ) {
+     exit
+}
+
 pause -1
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 9d431b7..8474bb8 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -154,9 +154,9 @@
   }
 
   void FreeBufferIndex(int i) {
-    // This extra checking has a large performance hit with msan, so just skip
-    // it.
-#if !__has_feature(memory_sanitizer)
+    // This extra checking has a large performance hit with sanitizers that
+    // track memory accesses, so just skip it.
+#if !__has_feature(memory_sanitizer) && !__has_feature(address_sanitizer)
     DCHECK(std::find(available_buffer_indices_.begin(),
                      available_buffer_indices_.end(),
                      i) == available_buffer_indices_.end())
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index 052fa9c..1f475be 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -9,6 +9,7 @@
 #include "aos/fast_string_builder.h"
 #include "aos/flatbuffer_utils.h"
 #include "aos/flatbuffers.h"
+#include "aos/util/file.h"
 #include "flatbuffers/flatbuffers.h"
 #include "flatbuffers/reflection.h"
 
@@ -111,13 +112,10 @@
 // Parses a file as a binary flatbuffer or dies.
 template <typename T>
 inline FlatbufferVector<T> FileToFlatbuffer(const std::string_view path) {
-  std::ifstream instream(std::string(path), std::ios::in | std::ios::binary);
+  const std::string data_string = util::ReadFileToStringOrDie(path);
   ResizeableBuffer data;
-  std::istreambuf_iterator<char> it(instream);
-  while (it != std::istreambuf_iterator<char>()) {
-    data.push_back(*it);
-    ++it;
-  }
+  data.resize(data_string.size());
+  memcpy(data.data(), data_string.data(), data_string.size());
   return FlatbufferVector<T>(std::move(data));
 }
 
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index 9dc12d2..aa259df 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -79,10 +79,20 @@
   EXPECT_TRUE(JsonAndBack("{ \"foo_enum_nonconsecutive\": \"Big\" }"));
 }
 
+// Tests that Inf is handled correctly
+TEST_F(JsonToFlatbufferTest, Inf) {
+  EXPECT_TRUE(JsonAndBack("{ \"foo_float\": inf }"));
+  EXPECT_TRUE(JsonAndBack("{ \"foo_float\": -inf }"));
+  EXPECT_TRUE(JsonAndBack("{ \"foo_double\": inf }"));
+  EXPECT_TRUE(JsonAndBack("{ \"foo_double\": -inf }"));
+}
+
 // Tests that NaN is handled correctly
 TEST_F(JsonToFlatbufferTest, Nan) {
   EXPECT_TRUE(JsonAndBack("{ \"foo_float\": nan }"));
   EXPECT_TRUE(JsonAndBack("{ \"foo_float\": -nan }"));
+  EXPECT_TRUE(JsonAndBack("{ \"foo_double\": nan }"));
+  EXPECT_TRUE(JsonAndBack("{ \"foo_double\": -nan }"));
 }
 
 // Tests that we can handle decimal points.
diff --git a/aos/json_tokenizer.cc b/aos/json_tokenizer.cc
index 9403daa..47fcdbe 100644
--- a/aos/json_tokenizer.cc
+++ b/aos/json_tokenizer.cc
@@ -152,6 +152,12 @@
     return true;
   }
 
+  // Inf is also acceptable.
+  if (Consume("inf")) {
+    *s = ::std::string(original.substr(0, original.size() - data_.size()));
+    return true;
+  }
+
   // Then, we either get a 0, or we get a nonzero.  Only nonzero can be followed
   // by a second number.
   if (!Consume("0")) {
@@ -463,6 +469,14 @@
     return true;
   }
 
+  if (field_value() == "inf") {
+    *value = std::numeric_limits<double>::infinity();
+    return true;
+  } else if (field_value() == "-inf") {
+    *value = -std::numeric_limits<double>::infinity();
+    return true;
+  }
+
   *value = strtod(field_value().c_str(), const_cast<char **>(&pos));
 
   if (pos != field_value().c_str() + field_value().size() || errno != 0) {
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 4b6a44b..292d24d 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -17,14 +17,17 @@
             "of CSV files in /tmp/.  This should only be needed when debugging "
             "time synchronization.");
 
-DEFINE_int32(max_invalid_distance_ns, 500,
+DEFINE_int32(max_invalid_distance_ns, 0,
              "The max amount of time we will let the solver go backwards.");
 
 namespace aos {
 namespace message_bridge {
 namespace {
 namespace chrono = std::chrono;
-}
+
+const Eigen::IOFormat kHeavyFormat(Eigen::StreamPrecision, Eigen::DontAlignCols,
+                                   ", ", ";\n", "[", "]", "[", "]");
+}  // namespace
 
 TimestampProblem::TimestampProblem(size_t count) {
   CHECK_GT(count, 1u);
@@ -34,13 +37,15 @@
   node_mapping_.resize(count, 0);
 }
 
-// TODO(austin): Add linear inequality constraints too.
+// TODO(austin): Add linear inequality constraints too.  Currently we just
+// enforce them.
 //
 // TODO(austin): Add a rate of change constraint from the last sample.  1
 // ms/s.  Figure out how to define it.  Do this last.  This lets us handle
 // constraints going away, and constraints close in time.
 //
-// TODO(austin): Use the timestamp of the remote timestamp as more data.
+// TODO(austin): When the new newton's method solver prooves it's worth, kill
+// the old SLSQP solver.  It will be unreachable for a little bit.
 
 std::vector<double> TimestampProblem::SolveDouble() {
   MaybeUpdateNodeMapping();
@@ -146,6 +151,239 @@
   return success;
 }
 
+Eigen::VectorXd TimestampProblem::Gradient(
+    const Eigen::Ref<Eigen::VectorXd> time_offsets) const {
+  Eigen::VectorXd grad = Eigen::VectorXd::Zero(live_nodes_);
+  for (size_t i = 0; i < filters_.size(); ++i) {
+    for (const struct FilterPair &filter : filters_[i]) {
+      // Reminder, our cost function has the following form.
+      //   ((tb - (1 + ma) ta - ba)^2
+      // We are ignoring the slope when taking the derivative and applying the
+      // chain rule to keep the gradient smooth.  This means that the gradient
+      // is +- 2 * error.
+      //
+      const size_t a_solution_index = NodeToFullSolutionIndex(i);
+      const size_t b_solution_index = NodeToFullSolutionIndex(filter.b_index);
+      const double error =
+          2.0 * filter.filter->OffsetError(base_clock_[i],
+                                           time_offsets(a_solution_index),
+                                           base_clock_[filter.b_index],
+                                           time_offsets(b_solution_index));
+
+      grad(a_solution_index) += -error;
+      grad(b_solution_index) += error;
+    }
+  }
+  return grad;
+}
+
+Eigen::MatrixXd TimestampProblem::Hessian(
+    const Eigen::Ref<Eigen::VectorXd> /*time_offsets*/) const {
+  Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(live_nodes_, live_nodes_);
+
+  for (size_t i = 0; i < filters_.size(); ++i) {
+    for (const struct FilterPair &filter : filters_[i]) {
+      // Reminder, our cost function has the following form.
+      //   ((tb - (1 + ma) ta - ba)^2
+      // We are ignoring the slope when taking the derivative and applying the
+      // chain rule to keep the gradient smooth.  This means that the Hessian is
+      // 2 for d^2 cost/dta^2 and d^2 cost/dtb^2
+      const size_t a_solution_index = NodeToFullSolutionIndex(i);
+      const size_t b_solution_index = NodeToFullSolutionIndex(filter.b_index);
+      hessian(a_solution_index, a_solution_index) += 2;
+      hessian(b_solution_index, a_solution_index) += -2;
+      hessian(a_solution_index, b_solution_index) =
+          hessian(b_solution_index, a_solution_index);
+      hessian(b_solution_index, b_solution_index) += 2;
+    }
+  }
+
+  return hessian;
+}
+
+Eigen::VectorXd TimestampProblem::Newton(
+    const Eigen::Ref<Eigen::VectorXd> time_offsets) const {
+  CHECK_GT(live_nodes_, 0u) << ": No live nodes to solve for.";
+  // TODO(austin): Each of the DCost functions does a binary search of the
+  // timestamps list.  By the time we have computed the gradient and Hessian,
+  // we've done 5 binary searches for the same information.
+  const Eigen::VectorXd grad = Gradient(time_offsets);
+  const Eigen::MatrixXd hessian = Hessian(time_offsets);
+  const Eigen::MatrixXd constraint_jacobian =
+      Eigen::MatrixXd::Ones(1, live_nodes_) / static_cast<double>(live_nodes_);
+  // https://www.cs.purdue.edu/homes/jhonorio/16spring-cs52000-equality.pdf
+  //
+  // Queue long explanation for why this is the right math...
+  //
+  // Our cost function is piecewise quadratic and simple by design.  It should
+  // also be convex.  This means it is equivalent for us to drive the gradient
+  // to 0 to find the corresponding times on all nodes.
+  //
+  // This gets us close but doesn't let us solve for the time corresponding to a
+  // specific time on one node on all the nodes.
+  //
+  // To do this, we want a Newton solver which works for equality constraints.
+  //   argmin f(x) subject to {A x = b}
+  // More specifically, we want a version of this which will start with
+  // infeasible initial solutions.
+  //
+  // The newton step for this is
+  //   X(n+1) = X(n) + xnt
+  //
+  //   [Hessian(cost(X(n))) A^T] [xnt] = [-grad(cost(X(n)))]
+  //   [A                     0] [w]     [-A X(n) + b      ]
+  //
+  // It turns out that w is the dual newton step.  But we don't actually need to
+  // track the dual problem to solve our problem except to show that the dual
+  // problem has also converged.
+  //
+  // We could set A to [1, 0, 0, ...] and force a single clock to a specific
+  // time.  But, that will result in the optimal solution being different
+  // depending on which node is picked.
+  //
+  // Instead, let's solve for the average clock instead.  This will be always
+  // symmetric, and we can drive the goal for that clock to make any individual
+  // clock have the right time.  That would be a solver wrapped around a solver.
+  //
+  // Turns out, we can do that by combining the iterations.  If we set A to
+  // [1/n, 1/n, 1/n ...], and b to the distributed clock, that would drive our
+  // states such that the distributed clock will be what we want.  If we instead
+  // leave A the same, but set (-A X(n) + b) to be ( - [1, 0, 0...] * X(n) +
+  // goal_clock), we will drive the distributed clock to be what it needs to be
+  // to set a node's clock to the right time.
+  //
+  // This ends up working surprisingly well.  A toy problem with 2 line segments
+  // and 2 nodes converges in 2 iterations.
+  //
+  // TODO(austin): Maybe drive the distributed so we drive the min clock?  This
+  // will solve the for loop at the same time, making things faster.
+  //
+  //
+  // To ensure reliable convergence, we want to make 1 adjustment to the above
+  // problem statement.
+  //
+  // d cost/dta =>
+  //   2 * (tb - (1 + ma) ta - ba) * (-(1 + ma))
+  //
+  // This means that as you move between line segments with different slopes,
+  // you end up with step changes in the gradient.  Solvers like continuous
+  // derivatives.  But, we don't really care if this is an exact solution to the
+  // cost problem.  We just care that it is close to a solution to the cost
+  // problem and more importantly well behaved.
+  //
+  // The simple fix is to ignore the slope when applying the chain rule.  This
+  // makes the derivative just be the distance to the line, which is a
+  // continuous function.  Newtons method then converges really really easily
+  // every time.
+
+  Eigen::MatrixXd a;
+  a.resize(live_nodes_ + 1, live_nodes_ + 1);
+  a.block(0, 0, live_nodes_, live_nodes_) = hessian;
+  a.block(0, live_nodes_, live_nodes_, 1) = constraint_jacobian.transpose();
+  a.block(live_nodes_, 0, 1, live_nodes_) = constraint_jacobian;
+  a(live_nodes_, live_nodes_) = 0.0;
+
+  Eigen::VectorXd b = Eigen::VectorXd::Zero(live_nodes_ + 1);
+  b.block(0, 0, live_nodes_, 1) = -grad;
+
+  // Since we are driving the clock on the solution node to the base_clock, that
+  // is equivalent to driving the solution node's offset to 0.
+  b(live_nodes_) = -time_offsets(NodeToFullSolutionIndex(solution_node_));
+
+  return a.colPivHouseholderQr().solve(b);
+}
+
+std::vector<monotonic_clock::time_point> TimestampProblem::SolveNewton() {
+  constexpr int kMaxIterations = 200;
+  MaybeUpdateNodeMapping();
+  VLOG(1) << "Solving for node " << solution_node_ << " at "
+          << base_clock(solution_node_);
+  Eigen::VectorXd data = Eigen::VectorXd::Zero(live_nodes_);
+
+  int solution_number = 0;
+  while (true) {
+    Eigen::VectorXd step = Newton(data);
+
+    if (VLOG_IS_ON(1)) {
+      // Print out the gradient ignoring the component removed by the equality
+      // constraint.  This tells us what gradient we are depending to try to
+      // finish our solution.
+      const Eigen::MatrixXd constraint_jacobian =
+          Eigen::MatrixXd::Ones(1, live_nodes_) /
+          static_cast<double>(live_nodes_);
+      Eigen::VectorXd adjusted_grad =
+          Gradient(data) + step(live_nodes_) * constraint_jacobian.transpose();
+
+      VLOG(1) << "Adjusted grad " << solution_number << " -> "
+              << std::setprecision(12) << std::fixed << std::setfill(' ')
+              << adjusted_grad.transpose().format(kHeavyFormat);
+    }
+
+    VLOG(1) << "Step " << solution_number << " -> " << std::setprecision(12)
+            << std::fixed << std::setfill(' ')
+            << step.transpose().format(kHeavyFormat);
+    // We got there if the max step is small (this is strongly correlated to the
+    // gradient since the Hessian is constant), and our solution node's time is
+    // also close.
+    if (step.block(0, 0, live_nodes_, 1).lpNorm<Eigen::Infinity>() < 1e-4 &&
+        std::abs(data(NodeToFullSolutionIndex(solution_node_))) < 1e-4) {
+      break;
+    }
+
+    data += step.block(0, 0, live_nodes_, 1);
+
+    ++solution_number;
+
+    // We are doing all our math with both an int64 base and a double offset.
+    // This lets us handle large offsets while retaining precision down to the
+    // nanosecond easily.
+    //
+    // Some problems start out with a poor initial solution.  This is especially
+    // true for the first solution.  Because we control the solver, as we
+    // determine that the double is getting too big, we can move that
+    // information to the int64 base clock.  Threshold this to not be *too* big
+    // since it makes it hard to debug as the data keeps jumping around.
+    for (size_t j = 0; j < size(); ++j) {
+      const size_t solution_index = NodeToFullSolutionIndex(j);
+      if (j != solution_node_ && live(j) &&
+          std::abs(data(solution_index)) > 1000) {
+        int64_t dsolution =
+            static_cast<int64_t>(std::round(data(solution_index)));
+        base_clock_[j] += chrono::nanoseconds(dsolution);
+        data(solution_index) -= dsolution;
+      }
+    }
+
+    // And finally, don't let us iterate forever.  If it isn't converging,
+    // report back.
+    if (solution_number > kMaxIterations) {
+      break;
+    }
+  }
+
+  VLOG(1) << "Solving for node " << solution_node_ << " of "
+          << base_clock(solution_node_) << " in " << solution_number
+          << " cycles";
+  std::vector<monotonic_clock::time_point> result(size());
+  for (size_t i = 0; i < size(); ++i) {
+    if (live(i)) {
+      result[i] =
+          base_clock(i) + std::chrono::nanoseconds(static_cast<int64_t>(
+                              std::round(data(NodeToFullSolutionIndex(i)))));
+      VLOG(1) << "live  " << result[i] << " "
+              << data(NodeToFullSolutionIndex(i));
+    } else {
+      result[i] = monotonic_clock::min_time;
+      VLOG(1) << "dead  " << result[i];
+    }
+  }
+  if (solution_number > kMaxIterations) {
+    LOG(FATAL) << "Failed to converge.";
+  }
+
+  return result;
+}
+
 double TimestampProblem::Cost(const double *time_offsets, double *grad) {
   ++cost_call_count_;
 
@@ -998,7 +1236,7 @@
 }
 
 std::tuple<NoncausalTimestampFilter *,
-           std::vector<aos::monotonic_clock::time_point>>
+           std::vector<aos::monotonic_clock::time_point>, int>
 MultiNodeNoncausalOffsetEstimator::NextSolution(
     TimestampProblem *problem,
     const std::vector<aos::monotonic_clock::time_point> &base_times) {
@@ -1047,11 +1285,12 @@
 
       problem->set_solution_node(node_a_index);
       problem->set_base_clock(problem->solution_node(), next_node_time);
-      if (VLOG_IS_ON(1)) {
+      if (VLOG_IS_ON(2)) {
         problem->Debug();
       }
       // TODO(austin): Can we cache?  Solving is expensive.
-      std::vector<monotonic_clock::time_point> solution = problem->Solve();
+      std::vector<monotonic_clock::time_point> solution =
+          problem->SolveNewton();
 
       // Bypass checking if order validation is turned off.  This lets us dump a
       // CSV file so we can view the problem and figure out what to do.  The
@@ -1122,39 +1361,6 @@
                       << " -> " << (result_times[i] - solution[i]).count()
                       << "ns";
           }
-          // Since we found a problem with the solution, solve one problem per
-          // node, starting at the problem point.  This will show us any
-          // inconsistencies due to the problem phrasing and which node we
-          // solved from.
-          for (size_t a_index = 0; a_index < solution.size(); ++a_index) {
-            if (!problem->live(a_index)) {
-              continue;
-            }
-            for (size_t node_index = 0; node_index < solution.size();
-                 ++node_index) {
-              // Offset everything based on the elapsed time since the last
-              // solution on the node we are solving for.  The rate that time
-              // elapses should be ~1.
-              problem->set_base_clock(node_index, solution[node_index]);
-            }
-
-            problem->set_solution_node(a_index);
-            problem->Debug();
-            const std::vector<double> resolve_solution_double =
-                problem->SolveDouble();
-            problem->PrintSolution(resolve_solution_double);
-
-            const std::vector<monotonic_clock::time_point> resolve_solution =
-                problem->DoubleToMonotonic(resolve_solution_double.data());
-
-            LOG(INFO) << "Candidate solution for resolved node " << a_index
-                      << " is";
-            for (size_t i = 0; i < resolve_solution.size(); ++i) {
-              LOG(INFO) << "  " << resolve_solution[i] << " vs original "
-                        << solution[i] << " -> "
-                        << (resolve_solution[i] - solution[i]).count();
-            }
-          }
 
           if (skip_order_validation_) {
             next_node_filter->Consume();
@@ -1175,7 +1381,7 @@
       VLOG(1) << "  " << result_times[i];
     }
   }
-  return std::make_tuple(next_filter, std::move(result_times));
+  return std::make_tuple(next_filter, std::move(result_times), solution_index);
 }
 
 std::optional<std::tuple<distributed_clock::time_point,
@@ -1188,7 +1394,8 @@
   // Ok, now solve for the minimum time on each channel.
   std::vector<aos::monotonic_clock::time_point> result_times;
   NoncausalTimestampFilter *next_filter = nullptr;
-  std::tie(next_filter, result_times) =
+  int solution_node_index = 0;
+  std::tie(next_filter, result_times, solution_node_index) =
       NextSolution(&problem, last_monotonics_);
 
   CHECK(!all_done_);
@@ -1235,13 +1442,16 @@
   if (first_solution_) {
     std::vector<aos::monotonic_clock::time_point> resolved_times;
     NoncausalTimestampFilter *resolved_next_filter = nullptr;
+    int resolved_solution_node_index = 0;
 
     VLOG(1) << "Resolving with updated base times for accuracy.";
-    std::tie(resolved_next_filter, resolved_times) =
+    std::tie(resolved_next_filter, resolved_times,
+             resolved_solution_node_index) =
         NextSolution(&problem, result_times);
 
     first_solution_ = false;
     next_filter = resolved_next_filter;
+    solution_node_index = resolved_solution_node_index;
 
     // Force any unknown nodes to track the distributed clock (which starts at 0
     // too).
@@ -1265,23 +1475,34 @@
       case TimeComparison::kAfter:
         problem.Debug();
         for (size_t i = 0; i < result_times.size(); ++i) {
-          LOG(INFO) << "  " << last_monotonics_[i] << " vs " << result_times[i];
+          LOG(INFO) << "  " << last_monotonics_[i] << " vs " << result_times[i]
+                    << " -> " << (last_monotonics_[i] - result_times[i]).count()
+                    << "ns";
         }
-        LOG(FATAL) << "Found a solution before the last returned solution.";
+        LOG(FATAL)
+            << "Found a solution before the last returned solution on node "
+            << solution_node_index;
         break;
       case TimeComparison::kEq:
         return NextTimestamp();
-      case TimeComparison::kInvalid:
-        if (InvalidDistance(last_monotonics_, result_times) <
+      case TimeComparison::kInvalid: {
+        const chrono::nanoseconds invalid_distance =
+            InvalidDistance(last_monotonics_, result_times);
+        if (invalid_distance <
             chrono::nanoseconds(FLAGS_max_invalid_distance_ns)) {
           return NextTimestamp();
         }
+        LOG(INFO) << "Times can't be compared by " << invalid_distance.count()
+                  << "ns";
         CHECK_EQ(last_monotonics_.size(), result_times.size());
         for (size_t i = 0; i < result_times.size(); ++i) {
-          LOG(INFO) << "  " << last_monotonics_[i] << " vs " << result_times[i];
+          LOG(INFO) << "  " << last_monotonics_[i] << " vs " << result_times[i]
+                    << " -> " << (last_monotonics_[i] - result_times[i]).count()
+                    << "ns";
         }
-        LOG(FATAL) << "Found solutions which can't be ordered.";
-        break;
+        LOG(FATAL) << "Please investigate.  Use --max_invalid_distance_ns="
+                   << invalid_distance.count() << " to ignore this.";
+      } break;
     }
   }
 
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 0c07850..338a515 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -64,6 +64,10 @@
   // each node.
   std::vector<monotonic_clock::time_point> Solve();
 
+  // Solves the optimization problem phrased using the symmetric Netwon's method
+  // solver and returns the optimal time on each node.
+  std::vector<monotonic_clock::time_point> SolveNewton();
+
   // Returns the squared error for all of the offsets.
   // time_offsets is the offsets from the base_clock for every node (in order)
   // except the solution node.  It should be one element shorter than the number
@@ -126,6 +130,17 @@
     return reinterpret_cast<TimestampProblem *>(data)->Cost(time_offsets, grad);
   }
 
+  // Returns the Hessian of the cost function at time_offsets.
+  Eigen::MatrixXd Hessian(const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+  // Returns the gradient of the cost function at time_offsets.
+  Eigen::VectorXd Gradient(
+      const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+
+  // Returns the newton step of the timestamp problem.  The last term is the
+  // scalar on the equality constraint.  This needs to be removed from the
+  // solution to get the actual newton step.
+  Eigen::VectorXd Newton(const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+
   void MaybeUpdateNodeMapping() {
     if (node_mapping_valid_) {
       return;
@@ -139,21 +154,28 @@
         node_mapping_[i] = std::numeric_limits<size_t>::max();
       }
     }
+    live_nodes_ = live_node_index;
     node_mapping_valid_ = true;
   }
 
   // Converts from a node index to an index in the solution.
   size_t NodeToSolutionIndex(size_t node_index) const {
-    CHECK(node_mapping_valid_);
     CHECK_NE(node_index, solution_node_);
     // The solver is going to provide us a matrix with solution_node_ removed.
     // The indices of all nodes before solution_node_ are in the same spot, and
     // the indices of the nodes after solution node are shifted over.
-    size_t mapped_node_index = node_mapping_[node_index];
+    size_t mapped_node_index = NodeToFullSolutionIndex(node_index);
     return node_index < solution_node_ ? mapped_node_index
                                        : (mapped_node_index - 1);
   }
 
+  // Converts from a node index to an index in the solution without skipping the
+  // solution node.
+  size_t NodeToFullSolutionIndex(size_t node_index) const {
+    CHECK(node_mapping_valid_);
+    return node_mapping_[node_index];
+  }
+
   // Number of times Cost has been called for tracking.
   int cost_call_count_ = 0;
 
@@ -166,8 +188,12 @@
   std::vector<monotonic_clock::time_point> base_clock_;
   std::vector<bool> live_;
 
+  // True if both node_mapping_ and live_nodes_ are valid.
   bool node_mapping_valid_ = false;
+  // Mapping from a node index to an index in the solution.
   std::vector<size_t> node_mapping_;
+  // The number of live nodes there are.
+  size_t live_nodes_ = 0;
 
   // Filter and the node index it is referencing.
   //   filter->Offset(ta) + ta => t_(b_node);
@@ -345,7 +371,7 @@
   TimestampProblem MakeProblem();
 
   std::tuple<NoncausalTimestampFilter *,
-             std::vector<aos::monotonic_clock::time_point>>
+             std::vector<aos::monotonic_clock::time_point>, int>
   NextSolution(TimestampProblem *problem,
                const std::vector<aos::monotonic_clock::time_point> &base_times);
 
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 54ce35f..1d13601 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -481,6 +481,63 @@
   EXPECT_TRUE(time_converter.NextTimestamp());
 }
 
+// Tests that our Newtons method solver returns consistent answers for a simple
+// problem or two.  Also confirm that the residual error to the constraints
+// looks sane, meaning it is centered.
+TEST(TimestampProblemTest, SolveNewton) {
+  FlatbufferDetachedBuffer<Node> node_a_buffer =
+      JsonToFlatbuffer<Node>("{\"name\": \"test_a\"}");
+  const Node *const node_a = &node_a_buffer.message();
+
+  FlatbufferDetachedBuffer<Node> node_b_buffer =
+      JsonToFlatbuffer<Node>("{\"name\": \"test_b\"}");
+  const Node *const node_b = &node_b_buffer.message();
+
+  const monotonic_clock::time_point e = monotonic_clock::epoch();
+  const monotonic_clock::time_point ta = e + chrono::milliseconds(500);
+
+  // Setup a time problem with an interesting shape that isn't simple and
+  // parallel.
+  NoncausalTimestampFilter a(node_a, node_b);
+  a.Sample(e, chrono::milliseconds(1002));
+  a.Sample(e + chrono::milliseconds(1000), chrono::milliseconds(1001));
+  a.Sample(e + chrono::milliseconds(3000), chrono::milliseconds(999));
+
+  NoncausalTimestampFilter b(node_b, node_a);
+  b.Sample(e + chrono::milliseconds(1000), -chrono::milliseconds(999));
+  b.Sample(e + chrono::milliseconds(2000), -chrono::milliseconds(1000));
+  b.Sample(e + chrono::milliseconds(4000), -chrono::milliseconds(1002));
+
+  TimestampProblem problem(2);
+  problem.set_base_clock(0, ta);
+  problem.set_base_clock(1, e);
+  problem.set_solution_node(0);
+  problem.add_filter(0, &a, 1);
+  problem.add_filter(1, &b, 0);
+
+  problem.Debug();
+
+  problem.set_base_clock(0, e + chrono::seconds(1));
+  problem.set_base_clock(1, e);
+
+  problem.set_solution_node(0);
+  std::vector<monotonic_clock::time_point> result1 = problem.SolveNewton();
+
+  problem.set_base_clock(1, result1[1]);
+  problem.set_solution_node(1);
+  std::vector<monotonic_clock::time_point> result2 = problem.SolveNewton();
+
+  EXPECT_EQ(result1[0], e + chrono::seconds(1));
+  EXPECT_EQ(result1[0], result2[0]);
+  EXPECT_EQ(result1[1], result2[1]);
+
+  // Confirm that the error is almost equal for both directions.  The solution
+  // is an integer solution, so there will be a little bit of error left over.
+  EXPECT_NEAR(a.OffsetError(result1[0], 0.0, result1[1], 0.0) -
+                  b.OffsetError(result1[1], 0.0, result1[0], 0.0),
+              0.0, 0.5);
+}
+
 }  // namespace testing
 }  // namespace message_bridge
 }  // namespace aos
diff --git a/aos/network/timestamp_channel.cc b/aos/network/timestamp_channel.cc
index d022b60..bc8c1d9 100644
--- a/aos/network/timestamp_channel.cc
+++ b/aos/network/timestamp_channel.cc
@@ -42,7 +42,7 @@
       RemoteMessage::GetFullyQualifiedName(), name_, node_, true);
   if (shared_timestamp_channel != nullptr) {
     LOG(WARNING) << "Failed to find timestamp channel {\"name\": \""
-                 << split_timestamp_channel << "\", \"type\": \""
+                 << split_timestamp_channel_name << "\", \"type\": \""
                  << RemoteMessage::GetFullyQualifiedName()
                  << "\"}, falling back to old version.";
     return shared_timestamp_channel;
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 498f111..dd77f40 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -49,6 +49,18 @@
   *ta_base += ta_digits;
   *ta -= static_cast<double>(ta_digits.count());
 
+  // Sign, numerical precision wins again.
+  //   *ta_base=1000.300249970sec, *ta=-1.35525e-20
+  // We then promptly round this to
+  //   *ta_base=1000.300249969sec, *ta=1
+  // The 1.0 then breaks the LT assumption below, so we kersplat.
+  //
+  // Detect this case directly and move the 1.0 back into ta_base.
+  if (*ta == 1.0) {
+    *ta = 0.0;
+    *ta_base += chrono::nanoseconds(1);
+  }
+
   CHECK_GE(*ta, 0.0);
   CHECK_LT(*ta, 1.0);
 }
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index f8955c6..f10746f 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -75,6 +75,12 @@
         value = self.vol_input.get_value()
         self.drawing_area.points.setConstraint("VOLTAGE", value)
 
+    def input_combobox_choice(self, combo):
+        text = combo.get_active_text()
+        if text is not None:
+            print("Combo Clicked on: " + text)
+            set_field(text)
+
     def __init__(self):
         Gtk.Window.__init__(self)
 
@@ -187,8 +193,37 @@
         container.put(self.output_json, 210, 0)
         container.put(self.input_json, 320, 0)
 
-        self.show_all()
 
+        #Dropdown feature
+        self.label = Gtk.Label("Change Map:")
+        self.label.set_size_request(100,40)
+        container.put(self.label,430,0)
+
+        game_store = Gtk.ListStore(str)
+        games = [
+           "2020 Field",
+           "2019 Field",
+           "2021 Galactic Search ARed",
+           "2021 Galactic Search ABlue",
+           "2021 Galactic Search BRed",
+           "2021 Galactic Search BBlue",
+           "2021 AutoNav Barrel Racing",
+           "2021 AutoNav Slalom",
+           "2021 AutoNav Bounce",
+           ]
+
+        self.game_combo = Gtk.ComboBoxText()
+        self.game_combo.set_entry_text_column(0)
+        self.game_combo.connect("changed", self.input_combobox_choice)
+
+        for game in games:
+          self.game_combo.append_text(game)
+
+        self.game_combo.set_active(0)
+        self.game_combo.set_size_request(100,40)
+        container.put(self.game_combo,440,30)
+
+        self.show_all()
 
 window = GridWindow()
 RunApp()
diff --git a/y2020/BUILD b/y2020/BUILD
index cd001a1..024b6ac 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -152,6 +152,7 @@
         ":config_pi2",
         ":config_pi3",
         ":config_pi4",
+        ":config_pi5",
         ":config_roborio",
     ],
     flatbuffers = [
@@ -191,6 +192,7 @@
         "pi2",
         "pi3",
         "pi4",
+        "pi5",
     ]
 ]
 
@@ -289,5 +291,5 @@
         parameters = {"NUM": str(num)},
         target_compatible_with = ["@platforms//os:linux"],
     )
-    for num in range(1, 5)
+    for num in range(1, 6)
 ]
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 1b98592..75a8475 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -117,16 +117,6 @@
     ],
 )
 
-aos_config(
-    name = "replay_config",
-    src = "drivetrain_replay_config.json",
-    target_compatible_with = ["@platforms//os:linux"],
-    visibility = ["//visibility:public"],
-    deps = [
-        "//y2020:config",
-    ],
-)
-
 cc_test(
     name = "localizer_test",
     srcs = ["localizer_test.cc"],
@@ -150,8 +140,8 @@
     name = "drivetrain_replay_test",
     srcs = ["drivetrain_replay_test.cc"],
     data = [
-        ":replay_config",
-        "@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
+        "//y2020:config",
+        "@drivetrain_replay",
     ],
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_config.json b/y2020/control_loops/drivetrain/drivetrain_replay_config.json
deleted file mode 100644
index 987e55b..0000000
--- a/y2020/control_loops/drivetrain/drivetrain_replay_config.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
-  "channels": [
-    {
-      "name": "/drivetrain",
-      "type": "frc971.IMUValues",
-      "frequency": 2000,
-      "source_node": "roborio"
-    }
-  ],
-  "imports": [
-    "../../y2020.json"
-  ]
-}
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 2e6b327..e6a3f6e 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -22,9 +22,10 @@
 #include "y2020/control_loops/drivetrain/drivetrain_base.h"
 
 DEFINE_string(
-    logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
+    logfile,
+    "external/drivetrain_replay/",
     "Name of the logfile to read from.");
-DEFINE_string(config, "y2020/control_loops/drivetrain/replay_config.json",
+DEFINE_string(config, "y2020/config.json",
               "Name of the config file to replay using.");
 
 namespace y2020 {
@@ -36,7 +37,8 @@
  public:
   DrivetrainReplayTest()
       : config_(aos::configuration::ReadConfig(FLAGS_config)),
-        reader_(FLAGS_logfile, &config_.message()) {
+        reader_(aos::logger::SortParts(aos::logger::FindLogs(FLAGS_logfile)),
+                &config_.message()) {
     aos::network::OverrideTeamNumber(971);
 
     // TODO(james): Actually enforce not sending on the same buses as the
@@ -55,10 +57,6 @@
 
     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>(
@@ -70,25 +68,6 @@
     test_event_loop_ =
         reader_.event_loop_factory()->MakeEventLoop("drivetrain_test", roborio_);
 
-    // IMU readings used to be published out one at a time, but we now expect
-    // batches.  Batch them up to upgrade the data.
-    imu_sender_ =
-        test_event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain");
-    test_event_loop_->MakeWatcher(
-        "/drivetrain", [this](const frc971::IMUValues &values) {
-          aos::Sender<frc971::IMUValuesBatch>::Builder builder =
-              imu_sender_.MakeBuilder();
-          flatbuffers::Offset<frc971::IMUValues> values_offsets =
-              aos::CopyFlatBuffer(&values, builder.fbb());
-          flatbuffers::Offset<
-              flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
-              values_offset = builder.fbb()->CreateVector(&values_offsets, 1);
-          frc971::IMUValuesBatch::Builder imu_values_batch_builder =
-              builder.MakeBuilder<frc971::IMUValuesBatch>();
-          imu_values_batch_builder.add_readings(values_offset);
-          builder.Send(imu_values_batch_builder.Finish());
-        });
-
     status_fetcher_ = test_event_loop_->MakeFetcher<
         frc971::control_loops::drivetrain::Status>("/drivetrain");
   }
@@ -102,7 +81,6 @@
   std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
       drivetrain_;
   std::unique_ptr<aos::EventLoop> test_event_loop_;
-  aos::Sender<frc971::IMUValuesBatch> imu_sender_;
 
   aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
 };
@@ -116,7 +94,10 @@
   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.25);
+  EXPECT_NEAR(status_fetcher_->estimated_left_position(),
+              status_fetcher_->estimated_right_position(), 0.1);
+  EXPECT_LT(std::abs(status_fetcher_->x()),
+            std::abs(status_fetcher_->estimated_left_position()) / 2.0);
   // 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 3059bf1..32c8834 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -24,7 +24,7 @@
 }
 
 // Indices of the pis to use.
-const std::array<std::string, 3> kPisToUse{"pi1", "pi2", "pi3"};
+const std::array<std::string, 5> kPisToUse{"pi1", "pi2", "pi3", "pi4", "pi5"};
 
 // Calculates the pose implied by the camera target, just based on
 // distance/heading components.
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 5dbad02..c3b6464 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -65,7 +65,7 @@
                             right_encoder, 0, 0, 0, 0, 0, 0)
                                .finished(),
                            ekf_.P());
-  };
+  }
 
  private:
   // Storage for a single turret position data point.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index d195b5d..a3e26b5 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -131,13 +131,14 @@
         drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
         last_frame_(monotonic_now()) {
     event_loop_factory()->SetTimeConverter(&time_converter_);
-    CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 5);
+    CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 6);
     CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), pi1_), 1);
     time_converter_.AddMonotonic({monotonic_clock::epoch() + kPiTimeOffset,
                                   monotonic_clock::epoch() + kPiTimeOffset,
                                   monotonic_clock::epoch() + kPiTimeOffset,
                                   monotonic_clock::epoch() + kPiTimeOffset,
                                   monotonic_clock::epoch() + kPiTimeOffset,
+                                  monotonic_clock::epoch() + kPiTimeOffset,
                                   monotonic_clock::epoch()});
     set_team_id(frc971::control_loops::testing::kTeamNumber);
     set_battery_voltage(12.0);
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 1cf54a2..14f6ac9 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -364,14 +364,24 @@
   void set_feeder_falcon(
       ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
     feeder_falcon_ = ::std::move(t);
-    CHECK_EQ(ctre::phoenix::OKAY,
-             feeder_falcon_->ConfigSupplyCurrentLimit(
-                 {true, Values::kFeederSupplyCurrentLimit(),
-                  Values::kFeederSupplyCurrentLimit(), 0}));
-    CHECK_EQ(ctre::phoenix::OKAY,
-             feeder_falcon_->ConfigStatorCurrentLimit(
-                 {true, Values::kFeederStatorCurrentLimit(),
-                  Values::kFeederStatorCurrentLimit(), 0}));
+    {
+      auto result = feeder_falcon_->ConfigSupplyCurrentLimit(
+          {true, Values::kFeederSupplyCurrentLimit(),
+           Values::kFeederSupplyCurrentLimit(), 0});
+      if (result != ctre::phoenix::OKAY) {
+        LOG(WARNING) << "Failed to configure feeder supply current limit: "
+                     << result;
+      }
+    }
+    {
+      auto result = feeder_falcon_->ConfigStatorCurrentLimit(
+          {true, Values::kFeederStatorCurrentLimit(),
+           Values::kFeederStatorCurrentLimit(), 0});
+      if (result != ctre::phoenix::OKAY) {
+        LOG(WARNING) << "Failed to configure feeder stator current limit: "
+                     << result;
+      }
+    }
   }
 
   void set_washing_machine_control_panel_victor(
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 59f3fc0..70e51b0 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -17,6 +17,7 @@
     "y2020_pi2.json",
     "y2020_pi3.json",
     "y2020_pi4.json",
+    "y2020_pi5.json",
     "y2020_laptop.json"
   ]
 }
diff --git a/y2020/y2020_laptop.json b/y2020/y2020_laptop.json
index 9ebac58..936f2cd 100644
--- a/y2020/y2020_laptop.json
+++ b/y2020/y2020_laptop.json
@@ -101,6 +101,20 @@
       ]
     },
     {
+      "name": "/pi5/aos",
+      "type": "aos.message_bridge.Timestamp",
+      "source_node": "pi5",
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": ["laptop"],
+      "destination_nodes": [
+        {
+          "name": "laptop",
+          "priority": 1,
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
       "name": "/laptop/aos",
       "type": "aos.timing.Report",
       "source_node": "laptop",
@@ -119,7 +133,7 @@
       "name": "/laptop/aos",
       "type": "aos.message_bridge.ServerStatistics",
       "source_node": "laptop",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -166,6 +180,13 @@
           "timestamp_logger_nodes": ["laptop"]
         },
         {
+          "name": "pi5",
+          "priority": 1,
+          "time_to_live": 5000000,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": ["laptop"]
+        },
+        {
           "name": "roborio",
           "priority": 1,
           "time_to_live": 5000000,
@@ -175,7 +196,7 @@
       ]
     },
     {
-      "name": "/laptop/aos/remote_timestamps/roborio",
+      "name": "/laptop/aos/remote_timestamps/roborio/laptop/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "source_node": "laptop",
       "logger": "NOT_LOGGED",
@@ -184,7 +205,7 @@
       "max_size": 200
     },
     {
-      "name": "/laptop/aos/remote_timestamps/pi1",
+      "name": "/laptop/aos/remote_timestamps/pi1/laptop/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "source_node": "laptop",
       "logger": "NOT_LOGGED",
@@ -193,7 +214,7 @@
       "max_size": 200
     },
     {
-      "name": "/laptop/aos/remote_timestamps/pi2",
+      "name": "/laptop/aos/remote_timestamps/pi2/laptop/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "source_node": "laptop",
       "logger": "NOT_LOGGED",
@@ -202,7 +223,7 @@
       "max_size": 200
     },
     {
-      "name": "/laptop/aos/remote_timestamps/pi3",
+      "name": "/laptop/aos/remote_timestamps/pi3/laptop/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "source_node": "laptop",
       "logger": "NOT_LOGGED",
@@ -211,7 +232,16 @@
       "max_size": 200
     },
     {
-      "name": "/laptop/aos/remote_timestamps/pi4",
+      "name": "/laptop/aos/remote_timestamps/pi4/laptop/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "source_node": "laptop",
+      "logger": "NOT_LOGGED",
+      "frequency": 20,
+      "num_senders": 2,
+      "max_size": 200
+    },
+    {
+      "name": "/laptop/aos/remote_timestamps/pi5/laptop/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "source_node": "laptop",
       "logger": "NOT_LOGGED",
@@ -330,6 +360,34 @@
           "time_to_live": 5000000
         }
       ]
+    },
+    {
+      "name": "/pi5/camera",
+      "type": "frc971.vision.CameraImage",
+      "source_node": "pi5",
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": ["laptop"],
+      "destination_nodes": [
+        {
+          "name": "laptop",
+          "priority": 1,
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/pi5/camera",
+      "type": "frc971.vision.sift.ImageMatchResult",
+      "source_node": "pi5",
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": ["laptop"],
+      "destination_nodes": [
+        {
+          "name": "laptop",
+          "priority": 1,
+          "time_to_live": 5000000
+        }
+      ]
     }
   ],
   "maps": [
@@ -367,6 +425,9 @@
     },
     {
       "name": "pi4"
+    },
+    {
+      "name": "pi5"
     }
   ]
 }
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index 47d576a..61094be 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -34,7 +34,7 @@
       "name": "/pi{{ NUM }}/aos",
       "type": "aos.message_bridge.ServerStatistics",
       "source_node": "pi{{ NUM }}",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 2e29eee..2a204ae 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -46,7 +46,7 @@
       "name": "/roborio/aos",
       "type": "aos.message_bridge.ServerStatistics",
       "source_node": "roborio",
-      "frequency": 2,
+      "frequency": 10,
       "num_senders": 2
     },
     {
@@ -57,28 +57,42 @@
       "num_senders": 2
     },
     {
-      "name": "/roborio/aos/remote_timestamps/pi1",
+      "name": "/roborio/aos/remote_timestamps/laptop/roborio/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "frequency": 200,
       "logger": "NOT_LOGGED",
       "source_node": "roborio"
     },
     {
-      "name": "/roborio/aos/remote_timestamps/pi2",
+      "name": "/roborio/aos/remote_timestamps/pi1/roborio/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "frequency": 200,
       "logger": "NOT_LOGGED",
       "source_node": "roborio"
     },
     {
-      "name": "/roborio/aos/remote_timestamps/pi3",
+      "name": "/roborio/aos/remote_timestamps/pi2/roborio/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "frequency": 200,
       "logger": "NOT_LOGGED",
       "source_node": "roborio"
     },
     {
-      "name": "/roborio/aos/remote_timestamps/pi4",
+      "name": "/roborio/aos/remote_timestamps/pi3/roborio/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 200,
+      "logger": "NOT_LOGGED",
+      "source_node": "roborio"
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/pi4/roborio/aos/aos-message_bridge-Timestamp",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 200,
+      "logger": "NOT_LOGGED",
+      "source_node": "roborio"
+    },
+    {
+      "name": "/roborio/aos/remote_timestamps/pi5/roborio/aos/aos-message_bridge-Timestamp",
       "type": "aos.message_bridge.RemoteMessage",
       "frequency": 200,
       "logger": "NOT_LOGGED",
@@ -119,6 +133,13 @@
           "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
           "timestamp_logger_nodes": ["roborio"],
           "time_to_live": 5000000
+        },
+        {
+          "name": "pi5",
+          "priority": 1,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": ["roborio"],
+          "time_to_live": 5000000
         }
       ]
     },
@@ -192,47 +213,17 @@
     },
     {
       "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.Output",
+      "source_node": "roborio",
+      "frequency": 200,
+      "num_senders": 2
+    },
+    {
+      "name": "/drivetrain",
       "type": "frc971.control_loops.drivetrain.Status",
       "source_node": "roborio",
       "frequency": 200,
       "max_size": 2000,
-      "num_senders": 2,
-      "destination_nodes": [
-        {
-          "name": "pi1",
-          "priority": 5,
-          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
-          "timestamp_logger_nodes": ["roborio"],
-          "time_to_live": 5000000
-        },
-        {
-          "name": "pi2",
-          "priority": 5,
-          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
-          "timestamp_logger_nodes": ["roborio"],
-          "time_to_live": 5000000
-        },
-        {
-          "name": "pi3",
-          "priority": 5,
-          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
-          "timestamp_logger_nodes": ["roborio"],
-          "time_to_live": 5000000
-        },
-        {
-          "name": "pi4",
-          "priority": 5,
-          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
-          "timestamp_logger_nodes": ["roborio"],
-          "time_to_live": 5000000
-        }
-      ]
-    },
-    {
-      "name": "/drivetrain",
-      "type": "frc971.control_loops.drivetrain.Output",
-      "source_node": "roborio",
-      "frequency": 200,
       "num_senders": 2
     },
     {
@@ -352,6 +343,9 @@
     },
     {
       "name": "pi4"
+    },
+    {
+      "name": "pi5"
     }
   ]
 }