diff --git a/aos/BUILD b/aos/BUILD
index 9768753..b8468bd 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -157,7 +157,10 @@
     ],
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
+        "//aos:init",
+        "//aos/events:shm_event_loop",
         "//aos/time",
+        "//aos/util:top",
         "@com_github_google_glog//:glog",
     ],
 )
diff --git a/aos/dump_rtprio.cc b/aos/dump_rtprio.cc
index 8020857..d36a1c0 100644
--- a/aos/dump_rtprio.cc
+++ b/aos/dump_rtprio.cc
@@ -18,9 +18,14 @@
 #include <set>
 #include <string>
 
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/top.h"
 #include "aos/time/time.h"
 #include "glog/logging.h"
 
+DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+
 namespace {
 
 const char *policy_string(uint32_t policy) {
@@ -52,17 +57,7 @@
   return str.substr(0, str.size() - 1);
 }
 
-int find_pid_max() {
-  int r;
-  FILE *pid_max_file = fopen("/proc/sys/kernel/pid_max", "r");
-  PCHECK(pid_max_file != nullptr)
-      << ": Failed to open /proc/sys/kernel/pid_max";
-  CHECK_EQ(1, fscanf(pid_max_file, "%d", &r));
-  PCHECK(fclose(pid_max_file) == 0);
-  return r;
-}
-
-cpu_set_t find_all_cpus() {
+cpu_set_t FindAllCpus() {
   long nproc = sysconf(_SC_NPROCESSORS_CONF);
   PCHECK(nproc != -1);
   cpu_set_t r;
@@ -253,47 +248,63 @@
 
 }  // namespace
 
-int main() {
-  const int pid_max = find_pid_max();
-  const cpu_set_t all_cpus = find_all_cpus();
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
 
-  std::multiset<Thread> threads;
+  aos::ShmEventLoop event_loop(&config.message());
+  event_loop.SkipTimingReport();
+  event_loop.SkipAosLog();
+  aos::util::Top top(&event_loop);
+  top.set_track_top_processes(true);
 
-  for (int i = 0; i < pid_max; ++i) {
-    bool not_there = false;
+  const cpu_set_t all_cpus = FindAllCpus();
 
-    const cpu_set_t cpu_mask = find_cpu_mask(i, &not_there);
-    const sched_param param = find_sched_param(i, &not_there);
-    const int scheduler = find_scheduler(i, &not_there);
-    const ::std::string exe = find_exe(i, &not_there);
-    const int nice_value = find_nice_value(i, &not_there);
+  top.set_on_reading_update([&]() {
+    std::multiset<Thread> threads;
 
-    int ppid = 0, sid = 0;
-    read_stat(i, &ppid, &sid, &not_there);
+    for (const std::pair<const pid_t, aos::util::Top::ProcessReadings>
+             &reading : top.readings()) {
+      pid_t tid = reading.first;
+      bool not_there = false;
 
-    int pgrp = 0;
-    ::std::string name;
-    read_status(i, ppid, &pgrp, &name, &not_there);
+      const cpu_set_t cpu_mask = find_cpu_mask(tid, &not_there);
+      const sched_param param = find_sched_param(tid, &not_there);
+      const int scheduler = find_scheduler(tid, &not_there);
+      const ::std::string exe = find_exe(tid, &not_there);
+      const int nice_value = find_nice_value(tid, &not_there);
 
-    if (not_there) continue;
+      int ppid = 0, sid = 0;
+      read_stat(tid, &ppid, &sid, &not_there);
 
-    const char *cpu_mask_string =
-        CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+      int pgrp = 0;
+      ::std::string name;
+      read_status(tid, ppid, &pgrp, &name, &not_there);
 
-    threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
-                           .exe = exe,
-                           .name = name,
-                           .cpu_mask = cpu_mask_string,
-                           .nice_value = nice_value,
-                           .sched_priority = param.sched_priority,
-                           .tid = i,
-                           .pid = pgrp,
-                           .ppid = ppid,
-                           .sid = sid});
-  }
+      if (not_there) continue;
 
-  printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
-  for (const auto &t : threads) {
-    t.Print();
-  }
+      const char *cpu_mask_string =
+          CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+
+      threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
+                             .exe = exe,
+                             .name = name,
+                             .cpu_mask = cpu_mask_string,
+                             .nice_value = nice_value,
+                             .sched_priority = param.sched_priority,
+                             .tid = tid,
+                             .pid = pgrp,
+                             .ppid = ppid,
+                             .sid = sid});
+    }
+
+    printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
+    for (const auto &t : threads) {
+      t.Print();
+    }
+    event_loop.Exit();
+  });
+
+  event_loop.Run();
 }
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index 6b5b601..f5e9f51 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -115,11 +115,7 @@
     const monotonic_clock::time_point monotonic_now =
         event_loop_->monotonic_now();
     phased_loop_.Reset(monotonic_now);
-    Reschedule(
-        [this](monotonic_clock::time_point sleep_time) {
-          Schedule(sleep_time);
-        },
-        monotonic_now);
+    Reschedule(monotonic_now);
     // Reschedule here will count cycles elapsed before now, and then the
     // reschedule before running the handler will count the time that elapsed
     // then. So clear the count here.
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 471b625..23250e1 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -511,26 +511,61 @@
   Ftrace ftrace_;
 };
 
-// Interface for phased loops.  They are built on timers.
+// Interface for phased loops. They are built on timers.
 class PhasedLoopHandler {
  public:
   virtual ~PhasedLoopHandler();
 
-  // Sets the interval and offset.  Any changes to interval and offset only take
-  // effect when the handler finishes running.
-  void set_interval_and_offset(const monotonic_clock::duration interval,
-                               const monotonic_clock::duration offset) {
-    phased_loop_.set_interval_and_offset(interval, offset);
+  // Sets the interval and offset. Any changes to interval and offset only take
+  // effect when the handler finishes running or upon a call to Reschedule().
+  //
+  // The precise semantics of the monotonic_now parameter are defined in
+  // phased_loop.h. The way that it gets used in this interface is to control
+  // what the cycles elapsed counter will read on the following call to your
+  // handler. In an idealized simulation environment, if you call
+  // set_interval_and_offset() during the phased loop offset without setting
+  // monotonic_now, then you should always see a count of 1 on the next cycle.
+  //
+  // With the default behavior (this is called inside your handler and with
+  // monotonic_now = nullopt), the next phased loop call will occur at most
+  // interval time after the current time. Note that in many cases this will
+  // *not* be the preferred behavior (in most cases, you would likely aim to
+  // keep the average frequency of the calls reasonably consistent).
+  //
+  // A combination of the monotonic_now parameter and manually calling
+  // Reschedule() outside of the phased loop handler can be used to alter the
+  // behavior of cycles_elapsed. For the default behavior, you can set
+  // monotonic_now to the current time. If you call set_interval_and_offset()
+  // and Reschedule() with the same monotonic_now, that will cause the next
+  // callback to occur in the range  (monotonic_now, monotonic_now + interval]
+  // and get called with a count of 1 (if the event is actually able to happen
+  // when it is scheduled to). E.g., if you are just adjusting the phased loop
+  // offset (but not the interval) and want to maintain a consistent frequency,
+  // you may call these functions with monotonic_now = now + interval / 2.
+  void set_interval_and_offset(
+      const monotonic_clock::duration interval,
+      const monotonic_clock::duration offset,
+      std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt) {
+    phased_loop_.set_interval_and_offset(interval, offset, monotonic_now);
   }
 
-  // Sets and gets the name of the timer.  Set this if you want a descriptive
+  // Reruns the scheduler for the phased loop, scheduling the next callback at
+  // the next available time after monotonic_now. This allows
+  // set_interval_and_offset() to be called and have the changes take effect
+  // before the next handler finishes. This will have no effect if run during
+  // the phased loop's own handler.
+  void Reschedule(monotonic_clock::time_point monotonic_now) {
+    cycles_elapsed_ += phased_loop_.Iterate(monotonic_now);
+    Schedule(phased_loop_.sleep_time());
+  }
+
+  // Sets and gets the name of the timer. Set this if you want a descriptive
   // name in the timing report.
   void set_name(std::string_view name) { name_ = std::string(name); }
   const std::string_view name() const { return name_; }
 
  protected:
-  void Call(std::function<monotonic_clock::time_point()> get_time,
-            std::function<void(monotonic_clock::time_point)> schedule);
+  void Call(std::function<monotonic_clock::time_point()> get_time);
 
   PhasedLoopHandler(EventLoop *event_loop, std::function<void(int)> fn,
                     const monotonic_clock::duration interval,
@@ -539,12 +574,6 @@
  private:
   friend class EventLoop;
 
-  void Reschedule(std::function<void(monotonic_clock::time_point)> schedule,
-                  monotonic_clock::time_point monotonic_now) {
-    cycles_elapsed_ += phased_loop_.Iterate(monotonic_now);
-    schedule(phased_loop_.sleep_time());
-  }
-
   virtual void Schedule(monotonic_clock::time_point sleep_time) = 0;
 
   EventLoop *event_loop_;
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index 0c8e41e..07bd6d4 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -1922,6 +1922,298 @@
   EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
 }
 
+// Tests that a phased loop responds correctly to a changing offset; sweep
+// across a variety of potential offset changes, to ensure that we are
+// exercising a variety of potential cases.
+TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
+  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+  const int kCount = 5;
+
+  auto loop1 = MakePrimary();
+
+  std::vector<aos::monotonic_clock::duration> offset_options;
+  for (int ii = 0; ii < kCount; ++ii) {
+    offset_options.push_back(ii * kInterval / kCount);
+  }
+  std::vector<aos::monotonic_clock::duration> offset_sweep;
+  // Run over all the pair-wise combinations of offsets.
+  for (int ii = 0; ii < kCount; ++ii) {
+    for (int jj = 0; jj < kCount; ++jj) {
+      offset_sweep.push_back(offset_options.at(ii));
+      offset_sweep.push_back(offset_options.at(jj));
+    }
+  }
+
+  std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+  PhasedLoopHandler *phased_loop;
+
+  // Run kCount iterations.
+  size_t counter = 0;
+  phased_loop = loop1->AddPhasedLoop(
+      [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
+       offset_sweep](int count) {
+        EXPECT_EQ(count, 1);
+        expected_times.push_back(loop1->context().monotonic_event_time);
+
+        counter++;
+
+        if (counter == offset_sweep.size()) {
+          LOG(INFO) << "Exiting";
+          this->Exit();
+          return;
+        }
+
+        phased_loop->set_interval_and_offset(kInterval,
+                                             offset_sweep.at(counter));
+      },
+      kInterval, offset_sweep.at(0));
+
+  Run();
+  ASSERT_EQ(expected_times.size(), offset_sweep.size());
+  for (size_t ii = 1; ii < expected_times.size(); ++ii) {
+    EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
+  }
+}
+
+// Tests that a phased loop responds correctly to being rescheduled with now
+// equal to a time in the past.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
+  const chrono::milliseconds kOffset = chrono::milliseconds(400);
+  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+  auto loop1 = MakePrimary();
+
+  std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+  PhasedLoopHandler *phased_loop;
+
+  int expected_count = 1;
+
+  // Set up a timer that will get run immediately after the phased loop and
+  // which will attempt to reschedule the phased loop to just before now. This
+  // should succeed, but will result in 0 cycles elapsing.
+  TimerHandler *manager_timer =
+      loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
+        if (expected_count == 0) {
+          LOG(INFO) << "Exiting";
+          this->Exit();
+          return;
+        }
+        phased_loop->Reschedule(loop1->context().monotonic_event_time -
+                                std::chrono::nanoseconds(1));
+        expected_count = 0;
+      });
+
+  phased_loop = loop1->AddPhasedLoop(
+      [&expected_count, &expected_times, &loop1, manager_timer](int count) {
+        EXPECT_EQ(count, expected_count);
+        expected_times.push_back(loop1->context().monotonic_event_time);
+
+        manager_timer->Setup(loop1->context().monotonic_event_time);
+      },
+      kInterval, kOffset);
+  phased_loop->set_name("Test loop");
+  manager_timer->set_name("Manager timer");
+
+  Run();
+
+  ASSERT_EQ(2u, expected_times.size());
+  ASSERT_EQ(expected_times[0], expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to being rescheduled at the time
+// when it should be triggering (it should kick the trigger to the next cycle).
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
+  const chrono::milliseconds kOffset = chrono::milliseconds(400);
+  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+  auto loop1 = MakePrimary();
+
+  std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+  PhasedLoopHandler *phased_loop;
+
+  bool should_exit = false;
+  // Set up a timer that will get run immediately after the phased loop and
+  // which will attempt to reschedule the phased loop to now. This should
+  // succeed, but will result in no change to the expected behavior (since this
+  // is the same thing that is actually done internally).
+  TimerHandler *manager_timer =
+      loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
+        if (should_exit) {
+          LOG(INFO) << "Exiting";
+          this->Exit();
+          return;
+        }
+        phased_loop->Reschedule(loop1->context().monotonic_event_time);
+        should_exit = true;
+      });
+
+  phased_loop = loop1->AddPhasedLoop(
+      [&expected_times, &loop1, manager_timer](int count) {
+        EXPECT_EQ(count, 1);
+        expected_times.push_back(loop1->context().monotonic_event_time);
+
+        manager_timer->Setup(loop1->context().monotonic_event_time);
+      },
+      kInterval, kOffset);
+  phased_loop->set_name("Test loop");
+  manager_timer->set_name("Manager timer");
+
+  Run();
+
+  ASSERT_EQ(2u, expected_times.size());
+  ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to being rescheduled at a time in
+// the distant future.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
+  const chrono::milliseconds kOffset = chrono::milliseconds(400);
+  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+  auto loop1 = MakePrimary();
+
+  std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+  PhasedLoopHandler *phased_loop;
+
+  bool should_exit = false;
+  int expected_count = 1;
+  TimerHandler *manager_timer = loop1->AddTimer(
+      [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
+        if (should_exit) {
+          LOG(INFO) << "Exiting";
+          this->Exit();
+          return;
+        }
+        expected_count = 10;
+        // Knock off 1 ns, since the scheduler rounds up when it is
+        // scheduled to exactly a loop time.
+        phased_loop->Reschedule(loop1->context().monotonic_event_time +
+                                kInterval * expected_count -
+                                std::chrono::nanoseconds(1));
+        should_exit = true;
+      });
+
+  phased_loop = loop1->AddPhasedLoop(
+      [&expected_times, &loop1, manager_timer, &expected_count](int count) {
+        EXPECT_EQ(count, expected_count);
+        expected_times.push_back(loop1->context().monotonic_event_time);
+
+        manager_timer->Setup(loop1->context().monotonic_event_time);
+      },
+      kInterval, kOffset);
+  phased_loop->set_name("Test loop");
+  manager_timer->set_name("Manager timer");
+
+  Run();
+
+  ASSERT_EQ(2u, expected_times.size());
+  ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to having its phase offset
+// incremented and then being scheduled after a set time, exercising a pattern
+// where a phased loop's offset is changed while trying to maintain the trigger
+// at a consistent period.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
+  const chrono::milliseconds kOffset = chrono::milliseconds(400);
+  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+  auto loop1 = MakePrimary();
+
+  std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+  PhasedLoopHandler *phased_loop;
+
+  bool should_exit = false;
+  TimerHandler *manager_timer = loop1->AddTimer(
+      [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
+        if (should_exit) {
+          LOG(INFO) << "Exiting";
+          this->Exit();
+          return;
+        }
+        // Schedule the next callback to be strictly later than the current time
+        // + interval / 2, to ensure a consistent frequency.
+        monotonic_clock::time_point half_time =
+            loop1->context().monotonic_event_time + kInterval / 2;
+        phased_loop->set_interval_and_offset(
+            kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
+        phased_loop->Reschedule(half_time);
+        should_exit = true;
+      });
+
+  phased_loop = loop1->AddPhasedLoop(
+      [&expected_times, &loop1, manager_timer](int count) {
+        EXPECT_EQ(1, count);
+        expected_times.push_back(loop1->context().monotonic_event_time);
+
+        manager_timer->Setup(loop1->context().monotonic_event_time);
+      },
+      kInterval, kOffset);
+  phased_loop->set_name("Test loop");
+  manager_timer->set_name("Manager timer");
+
+  Run();
+
+  ASSERT_EQ(2u, expected_times.size());
+  ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
+            expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to having its phase offset
+// decremented and then being scheduled after a set time, exercising a pattern
+// where a phased loop's offset is changed while trying to maintain the trigger
+// at a consistent period.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
+  const chrono::milliseconds kOffset = chrono::milliseconds(400);
+  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+  auto loop1 = MakePrimary();
+
+  std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+  PhasedLoopHandler *phased_loop;
+
+  bool should_exit = false;
+  TimerHandler *manager_timer = loop1->AddTimer(
+      [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
+        if (should_exit) {
+          LOG(INFO) << "Exiting";
+          this->Exit();
+          return;
+        }
+        // Schedule the next callback to be strictly later than the current time
+        // + interval / 2, to ensure a consistent frequency.
+        const aos::monotonic_clock::time_point half_time =
+            loop1->context().monotonic_event_time + kInterval / 2;
+        phased_loop->set_interval_and_offset(
+            kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
+        phased_loop->Reschedule(half_time);
+        should_exit = true;
+      });
+
+  phased_loop = loop1->AddPhasedLoop(
+      [&expected_times, &loop1, manager_timer](int count) {
+        EXPECT_EQ(1, count);
+        expected_times.push_back(loop1->context().monotonic_event_time);
+
+        manager_timer->Setup(loop1->context().monotonic_event_time);
+      },
+      kInterval, kOffset);
+  phased_loop->set_name("Test loop");
+  manager_timer->set_name("Manager timer");
+
+  Run();
+
+  ASSERT_EQ(2u, expected_times.size());
+  ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
+            expected_times[1]);
+}
+
 // Tests that senders count correctly in the timing report.
 TEST_P(AbstractEventLoopTest, SenderTimingReport) {
   FLAGS_timing_report_ms = 1000;
diff --git a/aos/events/event_loop_param_test.cc.rej b/aos/events/event_loop_param_test.cc.rej
new file mode 100644
index 0000000..c03b83d
--- /dev/null
+++ b/aos/events/event_loop_param_test.cc.rej
@@ -0,0 +1,300 @@
+diff a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc	(rejected hunks)
+@@ -1923,6 +1923,298 @@
+   EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
+ }
+ 
++// Tests that a phased loop responds correctly to a changing offset; sweep
++// across a variety of potential offset changes, to ensure that we are
++// exercising a variety of potential cases.
++TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
++  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++  const int kCount = 5;
++
++  auto loop1 = MakePrimary();
++
++  std::vector<aos::monotonic_clock::duration> offset_options;
++  for (int ii = 0; ii < kCount; ++ii) {
++    offset_options.push_back(ii * kInterval / kCount);
++  }
++  std::vector<aos::monotonic_clock::duration> offset_sweep;
++  // Run over all the pair-wise combinations of offsets.
++  for (int ii = 0; ii < kCount; ++ii) {
++    for (int jj = 0; jj < kCount; ++jj) {
++      offset_sweep.push_back(offset_options.at(ii));
++      offset_sweep.push_back(offset_options.at(jj));
++    }
++  }
++
++  std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++  PhasedLoopHandler *phased_loop;
++
++  // Run kCount iterations.
++  int counter = 0;
++  phased_loop = loop1->AddPhasedLoop(
++      [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
++       offset_sweep](int count) {
++        EXPECT_EQ(count, 1);
++        expected_times.push_back(loop1->context().monotonic_event_time);
++
++        counter++;
++
++        if (counter == offset_sweep.size()) {
++          LOG(INFO) << "Exiting";
++          this->Exit();
++          return;
++        }
++
++        phased_loop->set_interval_and_offset(kInterval,
++                                             offset_sweep.at(counter));
++      },
++      kInterval, offset_sweep.at(0));
++
++  Run();
++  ASSERT_EQ(expected_times.size(), offset_sweep.size());
++  for (size_t ii = 1; ii < expected_times.size(); ++ii) {
++    EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
++  }
++}
++
++// Tests that a phased loop responds correctly to being rescheduled with now
++// equal to a time in the past.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
++  const chrono::milliseconds kOffset = chrono::milliseconds(400);
++  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++  auto loop1 = MakePrimary();
++
++  std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++  PhasedLoopHandler *phased_loop;
++
++  int expected_count = 1;
++
++  // Set up a timer that will get run immediately after the phased loop and
++  // which will attempt to reschedule the phased loop to just before now. This
++  // should succeed, but will result in 0 cycles elapsing.
++  TimerHandler *manager_timer =
++      loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
++        if (expected_count == 0) {
++          LOG(INFO) << "Exiting";
++          this->Exit();
++          return;
++        }
++        phased_loop->Reschedule(loop1->context().monotonic_event_time -
++                                std::chrono::nanoseconds(1));
++        expected_count = 0;
++      });
++
++  phased_loop = loop1->AddPhasedLoop(
++      [&expected_count, &expected_times, &loop1, manager_timer](int count) {
++        EXPECT_EQ(count, expected_count);
++        expected_times.push_back(loop1->context().monotonic_event_time);
++
++        manager_timer->Setup(loop1->context().monotonic_event_time);
++      },
++      kInterval, kOffset);
++  phased_loop->set_name("Test loop");
++  manager_timer->set_name("Manager timer");
++
++  Run();
++
++  ASSERT_EQ(2u, expected_times.size());
++  ASSERT_EQ(expected_times[0], expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to being rescheduled at the time
++// when it should be triggering (it should kick the trigger to the next cycle).
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
++  const chrono::milliseconds kOffset = chrono::milliseconds(400);
++  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++  auto loop1 = MakePrimary();
++
++  std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++  PhasedLoopHandler *phased_loop;
++
++  bool should_exit = false;
++  // Set up a timer that will get run immediately after the phased loop and
++  // which will attempt to reschedule the phased loop to now. This should
++  // succeed, but will result in no change to the expected behavior (since this
++  // is the same thing that is actually done internally).
++  TimerHandler *manager_timer =
++      loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
++        if (should_exit) {
++          LOG(INFO) << "Exiting";
++          this->Exit();
++          return;
++        }
++        phased_loop->Reschedule(loop1->context().monotonic_event_time);
++        should_exit = true;
++      });
++
++  phased_loop = loop1->AddPhasedLoop(
++      [&expected_times, &loop1, manager_timer](int count) {
++        EXPECT_EQ(count, 1);
++        expected_times.push_back(loop1->context().monotonic_event_time);
++
++        manager_timer->Setup(loop1->context().monotonic_event_time);
++      },
++      kInterval, kOffset);
++  phased_loop->set_name("Test loop");
++  manager_timer->set_name("Manager timer");
++
++  Run();
++
++  ASSERT_EQ(2u, expected_times.size());
++  ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to being rescheduled at a time in
++// the distant future.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
++  const chrono::milliseconds kOffset = chrono::milliseconds(400);
++  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++  auto loop1 = MakePrimary();
++
++  std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++  PhasedLoopHandler *phased_loop;
++
++  bool should_exit = false;
++  int expected_count = 1;
++  TimerHandler *manager_timer = loop1->AddTimer(
++      [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
++        if (should_exit) {
++          LOG(INFO) << "Exiting";
++          this->Exit();
++          return;
++        }
++        expected_count = 10;
++        // Knock off 1 ns, since the scheduler rounds up when it is
++        // scheduled to exactly a loop time.
++        phased_loop->Reschedule(loop1->context().monotonic_event_time +
++                                kInterval * expected_count -
++                                std::chrono::nanoseconds(1));
++        should_exit = true;
++      });
++
++  phased_loop = loop1->AddPhasedLoop(
++      [&expected_times, &loop1, manager_timer, &expected_count](int count) {
++        EXPECT_EQ(count, expected_count);
++        expected_times.push_back(loop1->context().monotonic_event_time);
++
++        manager_timer->Setup(loop1->context().monotonic_event_time);
++      },
++      kInterval, kOffset);
++  phased_loop->set_name("Test loop");
++  manager_timer->set_name("Manager timer");
++
++  Run();
++
++  ASSERT_EQ(2u, expected_times.size());
++  ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to having its phase offset
++// incremented and then being scheduled after a set time, exercising a pattern
++// where a phased loop's offset is changed while trying to maintain the trigger
++// at a consistent period.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
++  const chrono::milliseconds kOffset = chrono::milliseconds(400);
++  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++  auto loop1 = MakePrimary();
++
++  std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++  PhasedLoopHandler *phased_loop;
++
++  bool should_exit = false;
++  TimerHandler *manager_timer = loop1->AddTimer(
++      [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
++        if (should_exit) {
++          LOG(INFO) << "Exiting";
++          this->Exit();
++          return;
++        }
++        // Schedule the next callback to be strictly later than the current time
++        // + interval / 2, to ensure a consistent frequency.
++        monotonic_clock::time_point half_time =
++            loop1->context().monotonic_event_time + kInterval / 2;
++        phased_loop->set_interval_and_offset(
++            kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
++        phased_loop->Reschedule(half_time);
++        should_exit = true;
++      });
++
++  phased_loop = loop1->AddPhasedLoop(
++      [&expected_times, &loop1, manager_timer](int count) {
++        EXPECT_EQ(1, count);
++        expected_times.push_back(loop1->context().monotonic_event_time);
++
++        manager_timer->Setup(loop1->context().monotonic_event_time);
++      },
++      kInterval, kOffset);
++  phased_loop->set_name("Test loop");
++  manager_timer->set_name("Manager timer");
++
++  Run();
++
++  ASSERT_EQ(2u, expected_times.size());
++  ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
++            expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to having its phase offset
++// decremented and then being scheduled after a set time, exercising a pattern
++// where a phased loop's offset is changed while trying to maintain the trigger
++// at a consistent period.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
++  const chrono::milliseconds kOffset = chrono::milliseconds(400);
++  const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++  auto loop1 = MakePrimary();
++
++  std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++  PhasedLoopHandler *phased_loop;
++
++  bool should_exit = false;
++  TimerHandler *manager_timer = loop1->AddTimer(
++      [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
++        if (should_exit) {
++          LOG(INFO) << "Exiting";
++          this->Exit();
++          return;
++        }
++        // Schedule the next callback to be strictly later than the current time
++        // + interval / 2, to ensure a consistent frequency.
++        const aos::monotonic_clock::time_point half_time =
++            loop1->context().monotonic_event_time + kInterval / 2;
++        phased_loop->set_interval_and_offset(
++            kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
++        phased_loop->Reschedule(half_time);
++        should_exit = true;
++      });
++
++  phased_loop = loop1->AddPhasedLoop(
++      [&expected_times, &loop1, manager_timer](int count) {
++        EXPECT_EQ(1, count);
++        expected_times.push_back(loop1->context().monotonic_event_time);
++
++        manager_timer->Setup(loop1->context().monotonic_event_time);
++      },
++      kInterval, kOffset);
++  phased_loop->set_name("Test loop");
++  manager_timer->set_name("Manager timer");
++
++  Run();
++
++  ASSERT_EQ(2u, expected_times.size());
++  ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
++            expected_times[1]);
++}
++
+ // Tests that senders count correctly in the timing report.
+ TEST_P(AbstractEventLoopTest, SenderTimingReport) {
+   gflags::FlagSaver flag_saver;
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index d206bc1..39ab43e 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -238,8 +238,7 @@
 }
 
 inline void PhasedLoopHandler::Call(
-    std::function<monotonic_clock::time_point()> get_time,
-    std::function<void(monotonic_clock::time_point)> schedule) {
+    std::function<monotonic_clock::time_point()> get_time) {
   // Read time directly to save a vtable indirection...
   const monotonic_clock::time_point monotonic_start_time = get_time();
 
@@ -270,7 +269,7 @@
   cycles_elapsed_ = 0;
 
   // Schedule the next wakeup.
-  schedule(phased_loop_.sleep_time());
+  Schedule(phased_loop_.sleep_time());
 
   const monotonic_clock::time_point monotonic_end_time = get_time();
   ftrace_.FormatMessage(
@@ -287,7 +286,7 @@
   // If the handler took too long so we blew by the previous deadline, we
   // want to just try for the next deadline.  Reschedule.
   if (monotonic_end_time > phased_loop_.sleep_time()) {
-    Reschedule(schedule, monotonic_end_time);
+    Reschedule(monotonic_end_time);
   }
 }
 
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index 56d2357..89d0a9c 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -679,9 +679,7 @@
     timerfd_.Read();
     event_.Invalidate();
 
-    Call(monotonic_clock::now, [this](monotonic_clock::time_point sleep_time) {
-      Schedule(sleep_time);
-    });
+    Call(monotonic_clock::now);
   }
 
   ~ShmPhasedLoopHandler() override {
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 22b9603..c679b21 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -1293,10 +1293,7 @@
   {
     ScopedMarkRealtimeRestorer rt(
         simulated_event_loop_->runtime_realtime_priority() > 0);
-    Call([monotonic_now]() { return monotonic_now; },
-         [this](monotonic_clock::time_point sleep_time) {
-           Schedule(sleep_time);
-         });
+    Call([monotonic_now]() { return monotonic_now; });
     simulated_event_loop_->ClearContext();
   }
 }
@@ -1312,6 +1309,7 @@
   // The allocations in here are due to infrastructure and don't count in the no
   // mallocs in RT code.
   ScopedNotRealtime nrt;
+  simulated_event_loop_->RemoveEvent(&event_);
   if (token_ != scheduler_->InvalidToken()) {
     scheduler_->Deschedule(token_);
     token_ = scheduler_->InvalidToken();
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 48dc054..929b376 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -466,6 +466,28 @@
 )
 
 cc_library(
+    name = "threaded_consumer",
+    hdrs = [
+        "threaded_consumer.h",
+    ],
+    deps = [
+        "//aos:condition",
+        "//aos:realtime",
+        "//aos/containers:ring_buffer",
+        "//aos/mutex",
+    ],
+)
+
+cc_test(
+    name = "threaded_consumer_test",
+    srcs = ["threaded_consumer_test.cc"],
+    deps = [
+        ":threaded_consumer",
+        "//aos/testing:googletest",
+    ],
+)
+
+cc_library(
     name = "foxglove_websocket_lib",
     srcs = ["foxglove_websocket_lib.cc"],
     hdrs = ["foxglove_websocket_lib.h"],
diff --git a/aos/util/phased_loop.cc b/aos/util/phased_loop.cc
index 53f6f4d..1f61775 100644
--- a/aos/util/phased_loop.cc
+++ b/aos/util/phased_loop.cc
@@ -17,15 +17,26 @@
 
 void PhasedLoop::set_interval_and_offset(
     const monotonic_clock::duration interval,
-    const monotonic_clock::duration offset) {
+    const monotonic_clock::duration offset,
+    std::optional<monotonic_clock::time_point> monotonic_now) {
   // Update last_time_ to the new offset so that we have an even interval
-  last_time_ += offset - offset_;
+  // In doing so, set things so that last_time_ will only ever decrease on calls
+  // to set_interval_and_offset.
+  last_time_ += offset - offset_ -
+                (offset > offset_ ? interval : monotonic_clock::duration(0));
 
   interval_ = interval;
   offset_ = offset;
   CHECK(offset_ >= monotonic_clock::duration(0));
   CHECK(interval_ > monotonic_clock::duration(0));
   CHECK(offset_ < interval_);
+  // Reset effectively clears the skipped iteration count and ensures that the
+  // last time is in the interval (monotonic_now - interval, monotonic_now],
+  // which means that a call to Iterate(monotonic_now) will return 1 and set a
+  // wakeup time after monotonic_now.
+  if (monotonic_now.has_value()) {
+    Iterate(monotonic_now.value());
+  }
 }
 
 monotonic_clock::duration PhasedLoop::OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop.h b/aos/util/phased_loop.h
index 307b400..64a3302 100644
--- a/aos/util/phased_loop.h
+++ b/aos/util/phased_loop.h
@@ -1,6 +1,8 @@
 #ifndef AOS_UTIL_PHASED_LOOP_H_
 #define AOS_UTIL_PHASED_LOOP_H_
 
+#include <optional>
+
 #include "aos/time/time.h"
 
 namespace aos {
@@ -21,8 +23,30 @@
       const monotonic_clock::duration offset = monotonic_clock::duration(0));
 
   // Updates the offset and interval.
-  void set_interval_and_offset(const monotonic_clock::duration interval,
-                               const monotonic_clock::duration offset);
+  //
+  // After a call to set_interval_and_offset with monotonic_now = nullopt, the
+  // following will hold, for any allowed values of interval and offset:
+  // auto original_time = loop.sleep_time();
+  // loop.set_interval_and_offset(interval, offset);
+  // CHECK_LE(loop.sleep_time(), original_time);
+  // CHECK_EQ(0, loop.Iterate(original_time));
+  //
+  // Note that this will not be the behavior that all (or even necessarily most)
+  // users want, since it doesn't necessarily preserve a "keep the iteration
+  // time as consistent as possible" concept. However, it *is* better defined
+  // than the alternative, where if you decrease the offset by, e.g., 1ms on a
+  // 100ms interval, then the behavior will vary depending on whather you are
+  // going from 0ms->999ms offset or from 1ms->0ms offset.
+  //
+  // If monotonic_now is set, then the following will hold:
+  // auto original_time = loop.sleep_time();
+  // loop.set_interval_and_offset(interval, offset, monotonic_now);
+  // CHECK_LE(loop.sleep_time(), monotonic_now);
+  // CHECK_EQ(0, loop.Iterate(monotonic_now));
+  void set_interval_and_offset(
+      const monotonic_clock::duration interval,
+      const monotonic_clock::duration offset,
+      std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt);
 
   // Computes the offset given an interval and a time that we should trigger.
   static monotonic_clock::duration OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 5e85ea0..de4483c 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -1,6 +1,7 @@
 #include "aos/util/phased_loop.h"
 
 #include "aos/time/time.h"
+#include "glog/logging.h"
 #include "gtest/gtest.h"
 
 namespace aos {
@@ -230,7 +231,23 @@
   ASSERT_EQ(5, loop.Iterate(last_time));
   for (int i = 1; i < kCount; i++) {
     const auto offset = kOffset - milliseconds(i);
-    loop.set_interval_and_offset(kInterval, offset);
+    // First, set the interval/offset without specifying a "now". If we then
+    // attempt to Iterate() to the same time as the last iteration, this should
+    // always result in zero cycles elapsed.
+    {
+      const monotonic_clock::time_point original_time = loop.sleep_time();
+      loop.set_interval_and_offset(kInterval, offset);
+      EXPECT_EQ(original_time - milliseconds(1), loop.sleep_time());
+      EXPECT_EQ(0, loop.Iterate(last_time));
+    }
+
+    // Now, explicitly update/clear things to last_time. This should have the
+    // same behavior as not specifying a monotonic_now.
+    {
+      loop.set_interval_and_offset(kInterval, offset, last_time);
+      EXPECT_EQ(0, loop.Iterate(last_time));
+    }
+
     const auto next_time = last_time - milliseconds(1) + kAllIterationsInterval;
     EXPECT_EQ(kIterations, loop.Iterate(next_time));
     last_time = next_time;
@@ -238,6 +255,43 @@
 }
 
 // Tests that the phased loop is correctly adjusting when the offset is
+// incremented multiple times.
+TEST_F(PhasedLoopTest, IncrementingOffset) {
+  constexpr int kCount = 5;
+  constexpr int kIterations = 10;
+  const auto kOffset = milliseconds(0);
+  const auto kInterval = milliseconds(1000);
+  const auto kAllIterationsInterval = kInterval * kIterations;
+
+  PhasedLoop loop(kInterval, monotonic_clock::epoch(), kOffset);
+  auto last_time = monotonic_clock::epoch() + kOffset + (kInterval * 3);
+  ASSERT_EQ(4, loop.Iterate(last_time));
+  for (int i = 1; i < kCount; i++) {
+    const auto offset = kOffset + milliseconds(i);
+    {
+      const monotonic_clock::time_point original_time = loop.sleep_time();
+      loop.set_interval_and_offset(kInterval, offset);
+      EXPECT_EQ(original_time - kInterval + milliseconds(1), loop.sleep_time());
+      EXPECT_EQ(0, loop.Iterate(last_time));
+    }
+    // Now, explicitly update/clear things to a set time. We add a milliseconds
+    // so that when we call Iterate() next we actually get the expected number
+    // of iterations (otherwise, there is an iteration that would happen at
+    // last_time + 1 that gets counted, which is correct behavior, and so just
+    // needs to be accounted for somehow).
+    {
+      loop.set_interval_and_offset(kInterval, offset,
+                                   last_time + milliseconds(1));
+      EXPECT_EQ(0, loop.Iterate(last_time + milliseconds(1)));
+    }
+
+    const auto next_time = last_time + milliseconds(1) + kAllIterationsInterval;
+    EXPECT_EQ(kIterations, loop.Iterate(next_time));
+    last_time = next_time;
+  }
+}
+
+// Tests that the phased loop is correctly adjusting when the offset is
 // changed to 0.
 TEST_F(PhasedLoopTest, ChangingOffset) {
   const auto kOffset = milliseconds(900);
diff --git a/aos/util/threaded_consumer.h b/aos/util/threaded_consumer.h
new file mode 100644
index 0000000..95ec79a
--- /dev/null
+++ b/aos/util/threaded_consumer.h
@@ -0,0 +1,102 @@
+#ifndef AOS_UTIL_THREADED_CONSUMER_H_
+#define AOS_UTIL_THREADED_CONSUMER_H_
+
+#include <functional>
+#include <optional>
+#include <thread>
+
+#include "aos/condition.h"
+#include "aos/containers/ring_buffer.h"
+#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
+
+namespace aos {
+namespace util {
+
+// This class implements a threadpool of a single worker that accepts work
+// from the main thread through a queue and executes it at a different realtime
+// priority.
+//
+// There is no mechanism to get data back to the main thread, the worker only
+// acts as a consumer. When this class is destroyed, it join()s the worker and
+// finishes all outstanding tasks.
+template <typename T, int QueueSize>
+class ThreadedConsumer {
+ public:
+  // Constructs a new ThreadedConsumer with the given consumer function to be
+  // run at the given realtime priority. If worker_priority is zero, the thread
+  // will stay at non realtime priority.
+  ThreadedConsumer(std::function<void(T)> consumer_function,
+                   int worker_priority)
+      : consumer_function_(consumer_function),
+        worker_priority_(worker_priority),
+        more_tasks_(&mutex_),
+        worker_thread_([this]() { WorkerFunction(); }) {}
+
+  ~ThreadedConsumer() {
+    {
+      aos::MutexLocker locker(&mutex_);
+      quit_ = true;
+      more_tasks_.Broadcast();
+    }
+    worker_thread_.join();
+  }
+
+  // Submits another task to be processed by the worker.
+  // Returns true if successfully pushed onto the queue, and false if the queue
+  // is full.
+  bool Push(T task) {
+    aos::MutexLocker locker(&mutex_);
+
+    if (task_queue_.full()) {
+      return false;
+    }
+
+    task_queue_.Push(task);
+    more_tasks_.Broadcast();
+
+    return true;
+  }
+
+ private:
+  void WorkerFunction() {
+    if (worker_priority_ > 0) {
+      aos::SetCurrentThreadRealtimePriority(worker_priority_);
+    }
+
+    while (true) {
+      std::optional<T> task;
+
+      {
+        aos::MutexLocker locker(&mutex_);
+        while (task_queue_.empty() && !quit_) {
+          CHECK(!more_tasks_.Wait());
+        }
+
+        if (task_queue_.empty() && quit_) break;
+
+        // Pop
+        task = std::move(task_queue_[0]);
+        task_queue_.Shift();
+      }
+
+      consumer_function_(*task);
+      task.reset();
+    }
+
+    aos::UnsetCurrentThreadRealtimePriority();
+  }
+
+  std::function<void(T)> consumer_function_;
+  aos::RingBuffer<T, QueueSize> task_queue_;
+  aos::Mutex mutex_;
+  bool quit_ = false;
+  int worker_priority_;
+  aos::Condition more_tasks_;
+  std::thread worker_thread_;
+};
+
+}  // namespace util
+}  // namespace aos
+
+#endif  // AOS_UTIL_THREADWORKER_H_
diff --git a/aos/util/threaded_consumer_test.cc b/aos/util/threaded_consumer_test.cc
new file mode 100644
index 0000000..f137108
--- /dev/null
+++ b/aos/util/threaded_consumer_test.cc
@@ -0,0 +1,144 @@
+#include "aos/util/threaded_consumer.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace util {
+
+// We expect it to be able to pass through everything we submit and recieves it
+// in the order that we submitted it. It should also be able to take in more
+// tasks than the size of the ring buffer as long as the worker doesn't get
+// behind.
+TEST(ThreadedConsumerTest, BasicFunction) {
+  std::atomic<int> counter{0};
+
+  ThreadedConsumer<int, 4> threaded_consumer(
+      [&counter](int task) {
+        LOG(INFO) << "task:" << task << " counter: " << counter;
+        counter = task;
+      },
+      0);
+
+  for (int number : {9, 7, 1, 3, 100, 300, 42}) {
+    EXPECT_TRUE(threaded_consumer.Push(number));
+
+    // wait
+    while (counter != number) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+    EXPECT_EQ(counter, number);
+  }
+}
+
+// We should be able to raise the realtime priority of the worker thread, and
+// everything should work the same. It should also reset back to lower priority
+// when shutting down the worker thread.
+TEST(ThreadedConsumerTest, ElevatedPriority) {
+  std::atomic<int> counter{0};
+
+  {
+    ThreadedConsumer<int, 4> threaded_consumer(
+        [&counter](int task) {
+          CheckRealtime();
+          LOG(INFO) << "task:" << task << " counter: " << counter;
+          counter = task;
+        },
+        20);
+
+    for (int number : {9, 7, 1, 3, 100, 300, 42}) {
+      EXPECT_TRUE(threaded_consumer.Push(number));
+
+      // wait
+      while (counter != number) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+      }
+
+      EXPECT_EQ(counter, number);
+    }
+  }
+  // TODO: Check that the worker thread's priority actually gets reset before
+  // the thread is destroyed.
+
+  CheckNotRealtime();
+}
+
+// If the worker gets behind, we shouldn't silently take in more tasks and
+// destroy old ones.
+TEST(ThreadedConsumerTest, OverflowRingBuffer) {
+  std::atomic<int> counter{0};
+  std::atomic<int> should_block{true};
+
+  ThreadedConsumer<int, 4> threaded_consumer(
+      [&counter, &should_block](int task) {
+        LOG(INFO) << "task:" << task << " counter: " << counter;
+
+        counter = task;
+
+        // prevent it from making any progress to simulate it getting behind
+        while (should_block) {
+          std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+      },
+      20);
+
+  // It consumes the 5 and then our worker blocks.
+  EXPECT_TRUE(threaded_consumer.Push(5));
+
+  // Wait for it to consume 5
+  while (counter != 5) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  };
+
+  // 4 more fills up the queue.
+  for (int number : {8, 9, 7, 1}) {
+    EXPECT_TRUE(threaded_consumer.Push(number));
+  }
+
+  // this one should overflow the buffer.
+  EXPECT_FALSE(threaded_consumer.Push(101));
+
+  // clean up, so we don't join() an infinite loop
+  should_block = false;
+}
+
+// The class should destruct gracefully and finish all of its work before
+// dissapearing.
+TEST(ThreadedConsumerTest, FinishesTasksOnQuit) {
+  std::atomic<int> counter{0};
+  std::atomic<int> should_block{true};
+
+  {
+    ThreadedConsumer<int, 4> threaded_consumer(
+        [&counter, &should_block](int task) {
+          LOG(INFO) << "task:" << task << " counter: " << counter;
+
+          counter = task;
+
+          // prevent it from making any progress to simulate it getting behind
+          while (should_block) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+          }
+        },
+        20);
+
+    // Give it some work to do
+    for (int number : {8, 9, 7, 1}) {
+      EXPECT_TRUE(threaded_consumer.Push(number));
+    }
+
+    // Wait for it to consume the first number
+    while (counter != 8) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    };
+
+    // allow it to continue
+    should_block = false;
+  }
+
+  // It should have finished all the work and gotten to the last number.
+  EXPECT_EQ(counter, 1);
+}
+
+}  // namespace util
+}  // namespace aos
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index 44e4f49..886a5d3 100755
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -7,7 +7,7 @@
 from gi.repository import Gdk
 from gi.repository import GdkX11
 import cairo
-from constants import *
+from frc971.control_loops.python.constants import *
 
 identity = cairo.Matrix()
 
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 80a4a53..2fed5f0 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -3,6 +3,7 @@
 from frc971.control_loops.python import control_loop
 from frc971.control_loops.python import controls
 import numpy
+import math
 import sys
 from matplotlib import pylab
 import glog
@@ -33,7 +34,8 @@
                  dt=0.00505,
                  controller_poles=[0.90, 0.90],
                  observer_poles=[0.02, 0.02],
-                 robot_cg_offset=0.0):
+                 robot_cg_offset=0.0,
+                 coefficient_of_friction=1.0):
         """Defines all constants of a drivetrain.
 
         Args:
@@ -107,6 +109,7 @@
         self.num_motors = num_motors
         self.controller_poles = controller_poles
         self.observer_poles = observer_poles
+        self.coefficient_of_friction = coefficient_of_friction
 
 
 class Drivetrain(control_loop.ControlLoop):
@@ -533,6 +536,221 @@
     kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
 
 
+def PlotDrivetrainSprint(drivetrain_params):
+    # Simulate the response of the system to a step input.
+    drivetrain = KFDrivetrain(left_low=False,
+                              right_low=False,
+                              drivetrain_params=drivetrain_params)
+    simulated_left_position = []
+    simulated_right_position = []
+    simulated_left_velocity = []
+    simulated_right_velocity = []
+
+    simulated_left_motor_currents = []
+    simulated_left_breaker_currents = []
+    simulated_right_motor_currents = []
+    simulated_right_breaker_currents = []
+
+    simulated_battery_heat_wattages = []
+    simulated_wattage = []
+    motor_inverter_voltages = []
+    voltage_left = []
+    voltage_right = []
+    simulated_motor_heat_wattages = []
+    simulated_motor_wattage = []
+
+    max_motor_currents = []
+    overall_currents = []
+    simulated_battery_wattage = []
+
+    # Distance in meters to call 1/2 field.
+    kSprintDistance = 8.0
+
+    vbat = 12.6
+    # Measured resistance of the battery, pd board, and breakers.
+    Rw = 0.023
+    top_speed = drivetrain.free_speed * (drivetrain.Gr +
+                                         drivetrain.Gl) / 2.0 * drivetrain.r
+
+    passed_distance = False
+    max_breaker_current = 0
+    heat_energy_usage = 0.0
+    for index in range(800):
+        # Current per side
+        left_traction_current = (drivetrain.mass / 2.0 *
+                                 drivetrain_params.coefficient_of_friction *
+                                 9.81 * drivetrain.r * drivetrain.Gl /
+                                 drivetrain.Kt)
+        right_traction_current = (drivetrain.mass / 2.0 *
+                                  drivetrain_params.coefficient_of_friction *
+                                  9.81 * drivetrain.r * drivetrain.Gr /
+                                  drivetrain.Kt)
+
+        # Detect if we've traveled over the sprint distance and report stats.
+        if (drivetrain.X[0, 0] + drivetrain.X[2, 0]) / 2.0 > kSprintDistance:
+            if not passed_distance:
+                velocity = (drivetrain.X[1, 0] + drivetrain.X[3, 0]) / 2.0
+                print("Took", index * drivetrain.dt,
+                      "to pass 1/2 field, going", velocity, "m/s,",
+                      velocity / 0.0254 / 12.0, "Traction limit current",
+                      left_traction_current / drivetrain_params.num_motors,
+                      "max breaker current", max_breaker_current, "top speed",
+                      top_speed, "m/s", top_speed / 0.0254 / 12.0,
+                      "fps, gear ratio", drivetrain.Gl, "heat energy",
+                      heat_energy_usage)
+            passed_distance = True
+
+        bemf_left = drivetrain.X[
+            1, 0] / drivetrain.r / drivetrain.Gl / drivetrain.Kv
+        bemf_right = drivetrain.X[
+            3, 0] / drivetrain.r / drivetrain.Gr / drivetrain.Kv
+
+        # Max current we could push through the motors is what we would get if
+        # we short the battery through the battery resistance into the motor.
+        max_motor_current = (vbat - (bemf_left + bemf_right) / 2.0) / (
+            Rw + drivetrain.resistance / 2.0)
+
+        max_motor_currents.append(max_motor_current /
+                                  (drivetrain_params.num_motors * 2))
+
+        # From this current, we can compute the voltage we can apply.
+        # This is either the traction limit or the current limit.
+        max_voltage_left = bemf_left + min(
+            max_motor_current / 2,
+            left_traction_current) * drivetrain.resistance
+        max_voltage_right = bemf_right + min(
+            max_motor_current / 2,
+            right_traction_current) * drivetrain.resistance
+
+        simulated_left_position.append(drivetrain.X[0, 0])
+        simulated_left_velocity.append(drivetrain.X[1, 0])
+        simulated_right_position.append(drivetrain.X[2, 0])
+        simulated_right_velocity.append(drivetrain.X[3, 0])
+
+        U = numpy.matrix([[min(max_voltage_left, vbat)],
+                          [min(max_voltage_right, vbat)]])
+
+        # Stator current
+        simulated_left_motor_current = (U[0, 0] -
+                                        bemf_left) / drivetrain.resistance
+        simulated_right_motor_current = (U[1, 0] -
+                                         bemf_right) / drivetrain.resistance
+
+        # And this gives us the power pushed into the motors.
+        power = (U[0, 0] * simulated_left_motor_current +
+                 U[1, 0] * simulated_right_motor_current)
+
+        simulated_wattage.append(power)
+
+        # Solve for the voltage we'd have to supply to the input of the motor
+        # controller to generate the power required.
+        motor_inverter_voltage = (
+            vbat + numpy.sqrt(vbat * vbat - 4.0 * power * Rw)) / 2.0
+
+        overall_current = (vbat - motor_inverter_voltage) / Rw
+        overall_currents.append(overall_current)
+
+        motor_inverter_voltages.append(motor_inverter_voltage)
+
+        # Overall left and right currents at the breaker
+        simulated_left_breaker_current = (
+            simulated_left_motor_current /
+            drivetrain_params.num_motors) * U[0, 0] / motor_inverter_voltage
+        simulated_right_breaker_current = (
+            simulated_right_motor_current /
+            drivetrain_params.num_motors) * U[1, 0] / motor_inverter_voltage
+
+        simulated_left_motor_currents.append(simulated_left_motor_current /
+                                             drivetrain_params.num_motors)
+        simulated_left_breaker_currents.append(simulated_left_breaker_current)
+        simulated_right_motor_currents.append(simulated_right_motor_current /
+                                              drivetrain_params.num_motors)
+        simulated_right_breaker_currents.append(
+            simulated_right_breaker_current)
+
+        # Save out the peak battery current observed.
+        max_breaker_current = max(
+            max_breaker_current,
+            max(simulated_left_breaker_current,
+                simulated_right_breaker_current))
+
+        # Compute the heat burned in the battery
+        simulated_battery_heat_wattage = math.pow(
+            vbat - motor_inverter_voltage, 2.0) / Rw
+        simulated_battery_heat_wattages.append(simulated_battery_heat_wattage)
+
+        motor_heat_wattage = (math.pow(simulated_left_motor_current, 2.0) *
+                              drivetrain.resistance +
+                              math.pow(simulated_right_motor_current, 2.0) *
+                              drivetrain.resistance)
+        simulated_motor_heat_wattages.append(motor_heat_wattage)
+
+        simulated_motor_wattage.append(simulated_left_motor_current * U[0, 0] +
+                                       simulated_right_motor_current * U[1, 0])
+
+        simulated_battery_wattage.append(vbat * overall_current)
+
+        # And then the overall energy outputted by the battery.
+        heat_energy_usage += (motor_heat_wattage +
+                              simulated_battery_heat_wattage) * drivetrain.dt
+
+        voltage_left.append(U[0, 0])
+        voltage_right.append(U[1, 0])
+
+        drivetrain.Update(U)
+
+    t = [drivetrain.dt * x for x in range(len(simulated_left_position))]
+    pylab.rc('lines', linewidth=4)
+    pylab.subplot(3, 1, 1)
+    pylab.plot(t, simulated_left_position, label='left position')
+    pylab.plot(t, simulated_right_position, 'r--', label='right position')
+    pylab.plot(t, simulated_left_velocity, label='left velocity')
+    pylab.plot(t, simulated_right_velocity, label='right velocity')
+
+    pylab.suptitle('Acceleration Test\n12 Volt Step Input')
+    pylab.legend(loc='lower right')
+
+    pylab.subplot(3, 1, 2)
+
+    pylab.plot(t, simulated_left_motor_currents, label='left rotor current')
+    pylab.plot(t,
+               simulated_right_motor_currents,
+               'r--',
+               label='right rotor current')
+    pylab.plot(t,
+               simulated_left_breaker_currents,
+               label='left breaker current')
+    pylab.plot(t,
+               simulated_right_breaker_currents,
+               'r--',
+               label='right breaker current')
+    pylab.plot(t, motor_inverter_voltages, label='motor inverter voltage')
+    pylab.plot(t, voltage_left, label='left voltage')
+    pylab.plot(t, voltage_right, label='right voltage')
+    pylab.plot(t, max_motor_currents, label='max_currents')
+    pylab.legend(loc='lower right')
+
+    wattage_axis = pylab.subplot(3, 1, 3)
+    wattage_axis.plot(t, simulated_wattage, label='wattage')
+    wattage_axis.plot(t,
+                      simulated_battery_heat_wattages,
+                      label='battery wattage')
+    wattage_axis.plot(t,
+                      simulated_motor_heat_wattages,
+                      label='motor heat wattage')
+    wattage_axis.plot(t, simulated_motor_wattage, label='motor wattage')
+    wattage_axis.plot(t, simulated_battery_wattage, label='overall wattage')
+    pylab.legend(loc='upper left')
+    overall_current_axis = wattage_axis.twinx()
+    overall_current_axis.plot(t, overall_currents, 'c--', label='current')
+
+    pylab.legend(loc='lower right')
+
+    pylab.suptitle('Acceleration Test\n12 Volt Step Input\n%f fps' %
+                   (top_speed / 0.0254 / 12.0, ))
+    pylab.show()
+
+
 def PlotDrivetrainMotions(drivetrain_params):
     # Test out the voltage error.
     drivetrain = KFDrivetrain(left_low=False,
diff --git a/frc971/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index 4bffe67..74d4dc1 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -186,7 +186,7 @@
 
 target "apt-get -y install -t bullseye-backports bpfcc-tools"
 
-target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo"
+target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo jq"
 target "cd /tmp && wget https://software.frc971.org/Build-Dependencies/libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && sudo dpkg -i libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && rm libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb"
 
 target "apt-get clean"
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 5d20aff..642ca0f 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -77,6 +77,7 @@
         "//aos/events:epoll",
         "//aos/events:event_loop",
         "//aos/scoped:scoped_fd",
+        "//aos/util:threaded_consumer",
         "@com_github_google_glog//:glog",
         "@com_google_absl//absl/base",
     ],
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 864c5e7..f12a6a5 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -27,7 +27,6 @@
     "The mininum number of aruco targets in charuco board required to match.");
 DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
 
-DEFINE_uint32(age, 100, "Age to start dropping frames at.");
 DEFINE_uint32(disable_delay, 100, "Time after an issue to disable tracing at.");
 
 DECLARE_bool(enable_ftrace);
@@ -94,8 +93,8 @@
 
 ImageCallback::ImageCallback(
     aos::EventLoop *event_loop, std::string_view channel,
-    std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn)
-
+    std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn,
+    monotonic_clock::duration max_age)
     : event_loop_(event_loop),
       server_fetcher_(
           event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
@@ -106,7 +105,8 @@
               ->source_node()
               ->string_view())),
       handle_image_(std::move(handle_image_fn)),
-      timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })) {
+      timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })),
+      max_age_(max_age) {
   event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
     const monotonic_clock::time_point eof_source_node =
         monotonic_clock::time_point(
@@ -145,7 +145,7 @@
     const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
     const double age_double =
         std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
-    if (age > std::chrono::milliseconds(FLAGS_age)) {
+    if (age > max_age_) {
       if (FLAGS_enable_ftrace) {
         ftrace_.FormatMessage("Too late receiving image, age: %f\n",
                               age_double);
@@ -230,12 +230,6 @@
     marker_length_ = 0.15;
     square_length_ = 0.1651;
     dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
-  } else if (target_type_ == TargetType::kAprilTag) {
-    // Tag will be 6 in (15.24 cm) according to
-    // https://www.firstinspires.org/robotics/frc/blog/2022-2023-approved-devices-rules-preview-and-vision-target-update
-    square_length_ = 0.1524;
-    dictionary_ =
-        cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
   } else {
     // Bail out if it's not a supported target
     LOG(FATAL) << "Target type undefined: "
@@ -435,9 +429,8 @@
                 << ", not enough marker IDs for charuco board, got "
                 << marker_ids.size() << ", needed " << FLAGS_min_charucos;
       }
-    } else if (target_type_ == TargetType::kAprilTag ||
-               target_type_ == TargetType::kAruco) {
-      // estimate pose for april tags doesn't return valid, so marking true
+    } else if (target_type_ == TargetType::kAruco) {
+      // estimate pose for arucos doesn't return valid, so marking true
       valid = true;
       std::vector<cv::Vec3d> rvecs, tvecs;
       cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index e296071..984cef6 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -55,13 +55,15 @@
     BGR = 1,
     GRAYSCALE = 2,
   };
-  ImageCallback(aos::EventLoop *event_loop, std::string_view channel,
-                std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
-                    &&handle_image_fn);
 
-  void set_format(Format format) {
-    format_ = format;
-  }
+  // `max_age` is the age to start dropping frames at
+  ImageCallback(
+      aos::EventLoop *event_loop, std::string_view channel,
+      std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
+          &&handle_image_fn,
+      aos::monotonic_clock::duration max_age = std::chrono::milliseconds(100));
+
+  void set_format(Format format) { format_ = format; }
 
  private:
   void DisableTracing();
@@ -77,14 +79,15 @@
   aos::Ftrace ftrace_;
 
   Format format_ = Format::BGR;
+
+  aos::monotonic_clock::duration max_age_;
 };
 
 // Types of targets that a CharucoExtractor can detect in images
 enum class TargetType : uint8_t {
-  kAprilTag = 0,
-  kAruco = 1,
-  kCharuco = 2,
-  kCharucoDiamond = 3
+  kAruco = 0,
+  kCharuco = 1,
+  kCharucoDiamond = 2
 };
 
 // Class which calls a callback each time an image arrives with the information
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 38bab6d..cee2b07 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -1,29 +1,44 @@
 namespace frc971.vision;
 
+table Position {
+  x:double (id: 0);
+  y:double (id: 1);
+  z:double (id: 2);
+}
+
+table Quaternion {
+  w:double (id: 0);
+  x:double (id: 1);
+  y:double (id: 2);
+  z:double (id: 3);
+}
+
 // Represents 3d pose of an april tag on the field.
 table TargetPoseFbs {
   // AprilTag ID of this target
   id:uint64 (id: 0);
 
-  // Pose of target relative to field origin.
-  // To get the pose of the target in the field frame, do:
-  // Translation3d(x, y, z) *
-  // (AngleAxisd(yaw) * AngleAxisd(pitch) * AngleAxisd(roll))
-  x:double (id: 1);
-  y:double (id: 2);
-  z:double (id: 3);
-
-  // Orientation of the target.
-  roll:double (id: 4);
-  pitch:double (id: 5);
-  yaw:double (id: 6);
+  // Pose of target relative to either the field origin or robot.
+  // To get the pose of the target, do:
+  // Translation3d(position.x(), position.y(), position.z()) *
+  // Quaterniond(orientation.w(), orientation.x(), orientation.y(), orientation.z())
+  position:Position (id: 1);
+  orientation:Quaternion (id: 2);
 }
 
 // Map of all target poses on a field.
-// This would be solved for by TargetMapper
+// There are two possible uses for this:
+// 1. Static april tag poses on the field solved for by TargetMapper.
+// 2. List of detected april poses relative to the robot.
 table TargetMap {
   target_poses:[TargetPoseFbs] (id: 0);
 
-  // Unique name of the field
+  // Unique name of the field (for use case 1.)
   field_name:string (id: 1);
-}
\ No newline at end of file
+
+  // End-of-frame timestamp for the frame with tag detections.
+  // (for use case 2.).
+  monotonic_timestamp_ns:int64 (id: 2);
+}
+
+root_type TargetMap;
diff --git a/frc971/vision/target_map.json b/frc971/vision/target_map.json
index 5f256fa..3f6eb54 100644
--- a/frc971/vision/target_map.json
+++ b/frc971/vision/target_map.json
@@ -2,75 +2,123 @@
     "target_poses": [
         {
             "id": 1,
-            "x": 7.244,
-            "y": -2.938,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.244,
+                "y": -2.938,
+                "z": 0.463
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 2,
-            "x": 7.244,
-            "y": -1.262,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.244,
+                "y": -1.262,
+                "z": 0.463
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 3,
-            "x": 7.244,
-            "y": 0.414,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.244,
+                "y": 0.414,
+                "z": 0.463
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 4,
-            "x": 7.909,
-            "y": 2.740,
-            "z": 0.695,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.909,
+                "y": 2.740,
+                "z": 0.695
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 5,
-            "x": -7.908,
-            "y": 2.740,
-            "z": 0.695,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.908,
+                "y": 2.740,
+                "z": 0.695
+            },
+            "orientation": {
+            /* yaw of 0 */
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         },
         {
             "id": 6,
-            "x": -7.243,
-            "y": 0.414,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.243,
+                "y": 0.414,
+                "z": 0.463
+            },
+            /* yaw of 0 */
+            "orientation": {
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         },
         {
             "id": 7,
-            "x": -7.243,
-            "y": -1.262,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.243,
+                "y": -1.262,
+                "z": 0.463
+            },
+            /* yaw of 0 */
+            "orientation": {
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         },
         {
             "id": 8,
-            "x": -7.243,
-            "y": -2.938,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.243,
+                "y": -2.938,
+                "z": 0.463
+            },
+            /* yaw of 0 */
+            "orientation": {
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         }
     ]
 }
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 5ce62a0..b3049e4 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -66,6 +66,32 @@
   return Eigen::Vector3d(roll, pitch, yaw);
 }
 
+flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs(
+    const TargetMapper::TargetPose &target_pose,
+    flatbuffers::FlatBufferBuilder *fbb) {
+  const auto position_offset =
+      CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
+                     target_pose.pose.p(2));
+  const auto orientation_offset =
+      CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
+                       target_pose.pose.q.y(), target_pose.pose.q.z());
+  return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
+                             orientation_offset);
+}
+
+TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs(
+    const TargetPoseFbs &target_pose_fbs) {
+  return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
+          .pose = ceres::examples::Pose3d{
+              Eigen::Vector3d(target_pose_fbs.position()->x(),
+                              target_pose_fbs.position()->y(),
+                              target_pose_fbs.position()->z()),
+              Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
+                                 target_pose_fbs.orientation()->x(),
+                                 target_pose_fbs.orientation()->y(),
+                                 target_pose_fbs.orientation()->z())}};
+}
+
 ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
     const std::vector<DataAdapter::TimestampedDetection>
         &timestamped_target_detections,
@@ -177,12 +203,8 @@
   aos::FlatbufferDetachedBuffer<TargetMap> target_map =
       aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
   for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
-    target_poses_[target_pose_fbs->id()] = ceres::examples::Pose3d{
-        Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
-                        target_pose_fbs->z()),
-        PoseUtils::EulerAnglesToQuaternion(
-            Eigen::Vector3d(target_pose_fbs->roll(), target_pose_fbs->pitch(),
-                            target_pose_fbs->yaw()))};
+    target_poses_[target_pose_fbs->id()] =
+        PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
   }
 }
 
@@ -215,7 +237,7 @@
     return;
   }
 
-  ceres::LossFunction *loss_function = NULL;
+  ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
   ceres::LocalParameterization *quaternion_local_parameterization =
       new ceres::EigenQuaternionParameterization;
 
@@ -253,12 +275,12 @@
   }
 
   // The pose graph optimization problem has six DOFs that are not fully
-  // constrained. This is typically referred to as gauge freedom. You can apply
-  // a rigid body transformation to all the nodes and the optimization problem
-  // will still have the exact same cost. The Levenberg-Marquardt algorithm has
-  // internal damping which mitigates this issue, but it is better to properly
-  // constrain the gauge freedom. This can be done by setting one of the poses
-  // as constant so the optimizer cannot change it.
+  // constrained. This is typically referred to as gauge freedom. You can
+  // apply a rigid body transformation to all the nodes and the optimization
+  // problem will still have the exact same cost. The Levenberg-Marquardt
+  // algorithm has internal damping which mitigates this issue, but it is
+  // better to properly constrain the gauge freedom. This can be done by
+  // setting one of the poses as constant so the optimizer cannot change it.
   ceres::examples::MapOfPoses::iterator pose_start_iter = poses->begin();
   CHECK(pose_start_iter != poses->end()) << "There are no poses.";
   problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
@@ -306,19 +328,8 @@
   // Convert poses to flatbuffers
   std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs;
   for (const auto &[id, pose] : target_poses_) {
-    TargetPoseFbs::Builder target_pose_builder(fbb);
-    target_pose_builder.add_id(id);
-
-    target_pose_builder.add_x(pose.p(0));
-    target_pose_builder.add_y(pose.p(1));
-    target_pose_builder.add_z(pose.p(2));
-
-    auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
-    target_pose_builder.add_roll(rpy.x());
-    target_pose_builder.add_pitch(rpy.y());
-    target_pose_builder.add_yaw(rpy.z());
-
-    target_poses_fbs.emplace_back(target_pose_builder.Finish());
+    target_poses_fbs.emplace_back(
+        PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
   }
 
   const auto field_name_offset = fbb.CreateString(field_name);
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 7e2c323..7701fd1 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -89,6 +89,14 @@
   static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
   // Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
   static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
+
+  // Builds a TargetPoseFbs from a TargetPose
+  static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
+      const TargetMapper::TargetPose &target_pose,
+      flatbuffers::FlatBufferBuilder *fbb);
+  // Converts a TargetPoseFbs to a TargetPose
+  static TargetMapper::TargetPose TargetPoseFromFbs(
+      const TargetPoseFbs &target_pose_fbs);
 };
 
 // Transforms robot position and target detection data into target constraints
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 9440813..271f787 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -408,15 +408,9 @@
 
   std::vector<TargetMapper::TargetPose> actual_target_poses;
   ceres::examples::MapOfPoses target_poses;
-  for (auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
-    auto target_pose = TargetMapper::TargetPose{
-        static_cast<int>(target_pose_fbs->id()),
-        ceres::examples::Pose3d{
-            Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
-                            target_pose_fbs->z()),
-            PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
-                target_pose_fbs->roll(), target_pose_fbs->pitch(),
-                target_pose_fbs->yaw()))}};
+  for (const auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
+    const TargetMapper::TargetPose target_pose =
+        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
     actual_target_poses.emplace_back(target_pose);
     target_poses[target_pose.id] = target_pose.pose;
   }
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index a6bcb4d..7c0546f 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -85,18 +85,23 @@
 
   for (size_t i = 0; i < buffers_.size(); ++i) {
     buffers_[i].sender = event_loop_->MakeSender<CameraImage>("/camera");
-    EnqueueBuffer(i);
+    MarkBufferToBeEnqueued(i);
   }
   int type = multiplanar() ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE
                            : V4L2_BUF_TYPE_VIDEO_CAPTURE;
   PCHECK(Ioctl(VIDIOC_STREAMON, &type) == 0);
 }
 
+void V4L2ReaderBase::MarkBufferToBeEnqueued(int buffer_index) {
+  ReinitializeBuffer(buffer_index);
+  EnqueueBuffer(buffer_index);
+}
+
 void V4L2ReaderBase::MaybeEnqueue() {
   // First, enqueue any old buffer we already have. This is the one which
   // may have been sent.
   if (saved_buffer_) {
-    EnqueueBuffer(saved_buffer_.index);
+    MarkBufferToBeEnqueued(saved_buffer_.index);
     saved_buffer_.Clear();
   }
   ftrace_.FormatMessage("Enqueued previous buffer %d", saved_buffer_.index);
@@ -114,7 +119,7 @@
       // going.
       if (previous_buffer) {
         ftrace_.FormatMessage("Previous %d", previous_buffer.index);
-        EnqueueBuffer(previous_buffer.index);
+        MarkBufferToBeEnqueued(previous_buffer.index);
       }
       continue;
     }
@@ -133,7 +138,12 @@
   }
 }
 
-void V4L2ReaderBase::SendLatestImage() { buffers_[saved_buffer_.index].Send(); }
+void V4L2ReaderBase::SendLatestImage() {
+  buffers_[saved_buffer_.index].Send();
+
+  MarkBufferToBeEnqueued(saved_buffer_.index);
+  saved_buffer_.Clear();
+}
 
 void V4L2ReaderBase::SetExposure(size_t duration) {
   v4l2_control manual_control;
@@ -236,7 +246,8 @@
 
   CHECK_GE(buffer_number, 0);
   CHECK_LT(buffer_number, static_cast<int>(buffers_.size()));
-  buffers_[buffer_number].InitializeMessage(ImageSize());
+  CHECK(buffers_[buffer_number].data_pointer != nullptr);
+
   struct v4l2_buffer buffer;
   struct v4l2_plane planes[1];
   memset(&buffer, 0, sizeof(buffer));
@@ -315,16 +326,21 @@
                                        const std::string &image_sensor_subdev)
     : V4L2ReaderBase(event_loop, device_name),
       epoll_(epoll),
-      image_sensor_fd_(open(image_sensor_subdev.c_str(), O_RDWR | O_NONBLOCK)) {
+      image_sensor_fd_(open(image_sensor_subdev.c_str(), O_RDWR | O_NONBLOCK)),
+      buffer_requeuer_([this](int buffer) { EnqueueBuffer(buffer); }, 20) {
   PCHECK(image_sensor_fd_.get() != -1)
       << " Failed to open device " << device_name;
-
   StreamOn();
   epoll_->OnReadable(fd().get(), [this]() { OnImageReady(); });
 }
 
 RockchipV4L2Reader::~RockchipV4L2Reader() { epoll_->DeleteFd(fd().get()); }
 
+void RockchipV4L2Reader::MarkBufferToBeEnqueued(int buffer) {
+  ReinitializeBuffer(buffer);
+  buffer_requeuer_.Push(buffer);
+}
+
 void RockchipV4L2Reader::OnImageReady() {
   if (!ReadLatestImage()) {
     return;
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 669c157..31c0888 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -5,10 +5,13 @@
 #include <string>
 
 #include "absl/types/span.h"
+#include "aos/containers/ring_buffer.h"
 #include "aos/events/epoll.h"
 #include "aos/events/event_loop.h"
 #include "aos/ftrace.h"
+#include "aos/realtime.h"
 #include "aos/scoped/scoped_fd.h"
+#include "aos/util/threaded_consumer.h"
 #include "frc971/vision/vision_generated.h"
 #include "glog/logging.h"
 
@@ -57,6 +60,23 @@
   void StreamOff();
   void StreamOn();
 
+  // Enqueues a buffer for v4l2 to stream into (expensive).
+  void EnqueueBuffer(int buffer_index);
+
+  // Initializations that need to happen in the main thread.
+  //
+  // Implementations of MarkBufferToBeEnqueued should call this before calling
+  // EnqueueBuffer.
+  void ReinitializeBuffer(int buffer_index) {
+    CHECK_GE(buffer_index, 0);
+    CHECK_LT(buffer_index, static_cast<int>(buffers_.size()));
+    buffers_[buffer_index].InitializeMessage(ImageSize());
+  }
+
+  // Submits a buffer to be enqueued later in a lower priority thread.
+  // Legacy V4L2Reader still does this in the main thread.
+  virtual void MarkBufferToBeEnqueued(int buffer_index);
+
   int Ioctl(unsigned long number, void *arg);
 
   bool multiplanar() const { return multiplanar_; }
@@ -70,9 +90,9 @@
 
   const aos::ScopedFD &fd() { return fd_; };
 
- private:
   static constexpr int kNumberBuffers = 4;
 
+ private:
   struct Buffer {
     void InitializeMessage(size_t max_image_size);
 
@@ -113,8 +133,6 @@
   // buffer, or BufferInfo() if there wasn't a frame to dequeue.
   BufferInfo DequeueBuffer();
 
-  void EnqueueBuffer(int buffer);
-
   // The mmaped V4L2 buffers.
   std::array<Buffer, kNumberBuffers> buffers_;
 
@@ -159,11 +177,15 @@
  private:
   void OnImageReady();
 
+  void MarkBufferToBeEnqueued(int buffer) override;
+
   int ImageSensorIoctl(unsigned long number, void *arg);
 
   aos::internal::EPoll *epoll_;
 
   aos::ScopedFD image_sensor_fd_;
+
+  aos::util::ThreadedConsumer<int, kNumberBuffers> buffer_requeuer_;
 };
 
 }  // namespace vision
diff --git a/third_party/apriltag/BUILD b/third_party/apriltag/BUILD
new file mode 100644
index 0000000..b11e34f
--- /dev/null
+++ b/third_party/apriltag/BUILD
@@ -0,0 +1,93 @@
+load("@//tools/build_rules:select.bzl", "compiler_select")
+
+cc_library(
+    name = "apriltag",
+    srcs = [
+        "apriltag.c",
+        "apriltag_pose.c",
+        "apriltag_quad_thresh.c",
+        "common/g2d.c",
+        "common/getopt.c",
+        "common/homography.c",
+        "common/image_u8.c",
+        "common/image_u8x3.c",
+        "common/image_u8x4.c",
+        "common/matd.c",
+        "common/pam.c",
+        "common/pjpeg.c",
+        "common/pjpeg-idct.c",
+        "common/pnm.c",
+        "common/string_util.c",
+        "common/svd22.c",
+        "common/time_util.c",
+        "common/unionfind.c",
+        "common/workerpool.c",
+        "common/zarray.c",
+        "common/zhash.c",
+        "common/zmaxheap.c",
+        "tag16h5.c",
+        "tag25h9.c",
+        "tag36h11.c",
+        "tagCircle21h7.c",
+        "tagCircle49h12.c",
+        "tagCustom48h12.c",
+        "tagStandard41h12.c",
+        "tagStandard52h13.c",
+    ],
+    hdrs = [
+        "apriltag.h",
+        "apriltag_math.h",
+        "apriltag_pose.h",
+        "common/debug_print.h",
+        "common/g2d.h",
+        "common/getopt.h",
+        "common/homography.h",
+        "common/image_types.h",
+        "common/image_u8.h",
+        "common/image_u8x3.h",
+        "common/image_u8x4.h",
+        "common/matd.h",
+        "common/math_util.h",
+        "common/pam.h",
+        "common/pjpeg.h",
+        "common/pnm.h",
+        "common/postscript_utils.h",
+        "common/pthreads_cross.h",
+        "common/string_util.h",
+        "common/svd22.h",
+        "common/time_util.h",
+        "common/timeprofile.h",
+        "common/unionfind.h",
+        "common/workerpool.h",
+        "common/zarray.h",
+        "common/zhash.h",
+        "common/zmaxheap.h",
+        "tag16h5.h",
+        "tag25h9.h",
+        "tag36h11.h",
+        "tagCircle21h7.h",
+        "tagCircle49h12.h",
+        "tagCustom48h12.h",
+        "tagStandard41h12.h",
+        "tagStandard52h13.h",
+    ],
+    copts = compiler_select({
+        "clang": [
+            "-Wno-cast-align",
+            "-Wno-incompatible-pointer-types-discards-qualifiers",
+        ],
+        "gcc": [
+            "-Wno-discarded-qualifiers",
+            "-Wno-shift-negative-value",
+        ],
+    }) + [
+        "-Wno-sign-compare",
+        "-Wno-cast-qual",
+        "-Wno-unused-parameter",
+        "-Wno-unused-variable",
+        "-Wno-format-nonliteral",
+    ],
+    includes = ["."],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+)
diff --git a/third_party/apriltag/common/workerpool.c b/third_party/apriltag/common/workerpool.c
index a0170ef..29eccfc 100644
--- a/third_party/apriltag/common/workerpool.c
+++ b/third_party/apriltag/common/workerpool.c
@@ -41,20 +41,6 @@
 #include "workerpool.h"
 #include "debug_print.h"
 
-struct workerpool {
-    int nthreads;
-    zarray_t *tasks;
-    int taskspos;
-
-    pthread_t *threads;
-    int *status;
-
-    pthread_mutex_t mutex;
-    pthread_cond_t startcond;   // used to signal the availability of work
-    pthread_cond_t endcond;     // used to signal completion of all work
-
-    int end_count; // how many threads are done?
-};
 
 struct task
 {
diff --git a/third_party/apriltag/common/workerpool.h b/third_party/apriltag/common/workerpool.h
index 2c32ab1..a233b5b 100644
--- a/third_party/apriltag/common/workerpool.h
+++ b/third_party/apriltag/common/workerpool.h
@@ -31,6 +31,21 @@
 
 typedef struct workerpool workerpool_t;
 
+struct workerpool {
+  int nthreads;
+  zarray_t *tasks;
+  int taskspos;
+
+  pthread_t *threads;
+  int *status;
+
+  pthread_mutex_t mutex;
+  pthread_cond_t startcond;  // used to signal the availability of work
+  pthread_cond_t endcond;    // used to signal completion of all work
+
+  int end_count;  // how many threads are done?
+};
+
 // as a special case, if nthreads==1, no additional threads are
 // created, and workerpool_run will run synchronously.
 workerpool_t *workerpool_create(int nthreads);
@@ -41,7 +56,8 @@
 // runs all added tasks, waits for them to complete.
 void workerpool_run(workerpool_t *wp);
 
-// same as workerpool_run, except always single threaded. (mostly for debugging).
+// same as workerpool_run, except always single threaded. (mostly for
+// debugging).
 void workerpool_run_single(workerpool_t *wp);
 
 int workerpool_get_nthreads(workerpool_t *wp);
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 6145452..d5ed54f 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -53,7 +53,8 @@
             [this](cv::Mat rgb_image,
                    const aos::monotonic_clock::time_point eof) {
               charuco_extractor_.HandleImage(rgb_image, eof);
-            }) {
+            },
+            std::chrono::milliseconds(5)) {
     CHECK(pi_number_) << ": Invalid pi number " << pi
                       << ", failed to parse pi number";
     std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 9b6c428..04b24ea 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -20,7 +20,7 @@
 DEFINE_bool(plot, false, "Whether to plot the resulting data.");
 DEFINE_bool(turret, true, "If true, the camera is on the turret");
 DEFINE_string(target_type, "charuco",
-              "Type of target: april_tag|aruco|charuco|charuco_diamond");
+              "Type of target: aruco|charuco|charuco_diamond");
 DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
 DEFINE_string(output_logs, "/tmp/calibration/",
               "Output folder for visualization logs.");
@@ -86,9 +86,7 @@
     logger.StartLoggingOnRun(FLAGS_output_logs);
 
     TargetType target_type = TargetType::kCharuco;
-    if (FLAGS_target_type == "april_tag") {
-      target_type = TargetType::kAprilTag;
-    } else if (FLAGS_target_type == "aruco") {
+    if (FLAGS_target_type == "aruco") {
       target_type = TargetType::kAruco;
     } else if (FLAGS_target_type == "charuco") {
       target_type = TargetType::kCharuco;
@@ -96,7 +94,7 @@
       target_type = TargetType::kCharucoDiamond;
     } else {
       LOG(FATAL) << "Unknown target type: " << FLAGS_target_type
-                 << ", expected: april_tag|aruco|charuco|charuco_diamond";
+                 << ", expected: aruco|charuco|charuco_diamond";
     }
 
     // Now, hook Calibration up to everything.
diff --git a/y2023/BUILD b/y2023/BUILD
index 0879de7..0c34ce8 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -7,7 +7,7 @@
     binaries = [
         "//y2020/vision:calibration",
         "//y2023/vision:viewer",
-        "//y2022/localizer:imu_main",
+        "//y2023/vision:aprilrobotics",
         "//y2022/localizer:localizer_main",
         "//aos/network:web_proxy_main",
         "//aos/events/logging:log_cat",
@@ -16,8 +16,6 @@
         ":aos_config",
         ":message_bridge_client.sh",
         "//y2022/www:www_files",
-        "@ctre_phoenix_api_cpp_athena//:shared_libraries",
-        "@ctre_phoenix_cci_athena//:shared_libraries",
     ],
     dirs = [
         "//y2023/www:www_files",
@@ -29,11 +27,6 @@
         "//aos/starter:irq_affinity",
         "//y2023/vision:camera_reader",
         "//aos/events/logging:logger_main",
-        ":joystick_reader",
-        "//y2023/autonomous:binaries",
-        "//y2023/control_loops/drivetrain:drivetrain",
-        "//y2023/control_loops/drivetrain:trajectory_generator",
-        "//y2023/control_loops/superstructure:superstructure",
     ],
     target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
     target_type = "pi",
@@ -48,6 +41,7 @@
         "//aos/network:timestamp_fbs",
         "//frc971/input:robot_state_fbs",
         "//frc971/vision:vision_fbs",
+        "//frc971/vision:target_map_fbs",
     ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
@@ -73,6 +67,7 @@
             "//aos/network:remote_message_fbs",
             "//y2022/localizer:localizer_output_fbs",
             "//frc971/vision:calibration_fbs",
+            "//frc971/vision:target_map_fbs",
             "//frc971/vision:vision_fbs",
         ],
         target_compatible_with = ["@platforms//os:linux"],
@@ -102,6 +97,7 @@
         "//y2022/localizer:localizer_status_fbs",
         "//y2022/localizer:localizer_output_fbs",
         "//y2022/localizer:localizer_visualization_fbs",
+        "//frc971/vision:target_map_fbs",
     ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
@@ -121,6 +117,7 @@
         "//aos/network:remote_message_fbs",
         "//frc971/vision:calibration_fbs",
         "//frc971/vision:vision_fbs",
+        "//frc971/vision:target_map_fbs",
     ],
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index b024676..a2ea6d8 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -31,6 +31,24 @@
     ],
 )
 
+py_binary(
+    name = "graph_edit",
+    srcs = [
+        "graph_edit.py",
+        "graph_generate.py",
+    ],
+    legacy_create_init = False,
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":python_init",
+        "//frc971/control_loops/python:basic_window",
+        "//frc971/control_loops/python:color",
+        "@pip//numpy",
+        "@pip//pygobject",
+        "@pip//shapely",
+    ],
+)
+
 py_library(
     name = "polydrivetrain_lib",
     srcs = [
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
new file mode 100644
index 0000000..7b6179c
--- /dev/null
+++ b/y2023/control_loops/python/graph_edit.py
@@ -0,0 +1,495 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+import os
+from frc971.control_loops.python import basic_window
+from frc971.control_loops.python.color import Color, palette
+import random
+import gi
+import numpy
+
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk, Gtk
+import cairo
+import graph_generate
+from graph_generate import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
+from graph_generate import back_to_xy_loop, subdivide_theta, to_theta_loop
+from graph_generate import l1, l2, joint_center
+
+from frc971.control_loops.python.basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+
+import shapely
+from shapely.geometry import Polygon
+
+
+def px(cr):
+    return OverrideMatrix(cr, identity)
+
+
+def draw_px_cross(cr, length_px):
+    """Draws a cross with fixed dimensions in pixel space."""
+    with px(cr):
+        x, y = cr.get_current_point()
+        cr.move_to(x, y - length_px)
+        cr.line_to(x, y + length_px)
+        cr.stroke()
+
+        cr.move_to(x - length_px, y)
+        cr.line_to(x + length_px, y)
+        cr.stroke()
+
+
+def angle_dist_sqr(a1, a2):
+    """Distance between two points in angle space."""
+    return (a1[0] - a2[0])**2 + (a1[1] - a2[1])**2
+
+
+# Find the highest y position that intersects the vertical line defined by x.
+def inter_y(x):
+    return numpy.sqrt((l2 + l1)**2 -
+                      (x - joint_center[0])**2) + joint_center[1]
+
+
+# This is the x position where the inner (hyperextension) circle intersects the horizontal line
+derr = numpy.sqrt((l1 - l2)**2 - (joint_center[1] - 0.3048)**2)
+
+
+# Define min and max l1 angles based on vertical constraints.
+def get_angle(boundary):
+    h = numpy.sqrt((l1)**2 - (boundary - joint_center[0])**2) + joint_center[1]
+    return numpy.arctan2(h, boundary - joint_center[0])
+
+
+# left hand side lines
+lines1 = [
+    (-0.826135, inter_y(-0.826135)),
+    (-0.826135, 0.1397),
+    (-23.025 * 0.0254, 0.1397),
+    (-23.025 * 0.0254, 0.3048),
+    (joint_center[0] - derr, 0.3048),
+]
+
+# right hand side lines
+lines2 = [(joint_center[0] + derr, 0.3048), (0.422275, 0.3048),
+          (0.422275, 0.1397), (0.826135, 0.1397),
+          (0.826135, inter_y(0.826135))]
+
+t1_min = get_angle((32.525 - 4.0) * 0.0254)
+t2_min = -7.0 / 4.0 * numpy.pi
+
+t1_max = get_angle((-32.525 + 4.0) * 0.0254)
+t2_max = numpy.pi * 3.0 / 4.0
+
+
+# Draw lines to cr + stroke.
+def draw_lines(cr, lines):
+    cr.move_to(lines[0][0], lines[0][1])
+    for pt in lines[1:]:
+        cr.line_to(pt[0], pt[1])
+    with px(cr):
+        cr.stroke()
+
+
+# Rotate a rasterized loop such that it aligns to when the parameters loop
+def rotate_to_jump_point(points):
+    last_pt = points[0]
+    for pt_i in range(1, len(points)):
+        pt = points[pt_i]
+        delta = last_pt[1] - pt[1]
+        if abs(delta) > numpy.pi:
+            return points[pt_i:] + points[:pt_i]
+        last_pt = pt
+    return points
+
+
+# shift points vertically by dy.
+def y_shift(points, dy):
+    return [(x, y + dy) for x, y in points]
+
+
+lines1_theta_part = rotate_to_jump_point(to_theta_loop(lines1, 0))
+lines2_theta_part = rotate_to_jump_point(to_theta_loop(lines2))
+
+# Some hacks here to make a single polygon by shifting to get an extra copy of the contraints.
+lines1_theta = y_shift(lines1_theta_part, -numpy.pi * 2) + lines1_theta_part + \
+    y_shift(lines1_theta_part, numpy.pi * 2)
+lines2_theta = y_shift(lines2_theta_part, numpy.pi * 2) + lines2_theta_part + \
+    y_shift(lines2_theta_part, -numpy.pi * 2)
+
+lines_theta = lines1_theta + lines2_theta
+
+p1 = Polygon(lines_theta)
+
+p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max),
+              (t1_min, t2_max)])
+
+# Fully computed theta constrints.
+lines_theta = list(p1.intersection(p2).exterior.coords)
+
+lines1_theta_back = back_to_xy_loop(lines1_theta)
+lines2_theta_back = back_to_xy_loop(lines2_theta)
+
+lines_theta_back = back_to_xy_loop(lines_theta)
+
+
+# Get the closest point to a line from a test pt.
+def get_closest(prev, cur, pt):
+    dx_ang = (cur[0] - prev[0])
+    dy_ang = (cur[1] - prev[1])
+
+    d = numpy.sqrt(dx_ang**2 + dy_ang**2)
+    if (d < 0.000001):
+        return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+
+    pdx = -dy_ang / d
+    pdy = dx_ang / d
+
+    dpx = pt[0] - prev[0]
+    dpy = pt[1] - prev[1]
+
+    alpha = (dx_ang * dpx + dy_ang * dpy) / d / d
+
+    if (alpha < 0):
+        return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+    elif (alpha > 1):
+        return cur, numpy.sqrt((cur[0] - pt[0])**2 + (cur[1] - pt[1])**2)
+    else:
+        return (alpha_blend(prev[0], cur[0], alpha), alpha_blend(prev[1], cur[1], alpha)), \
+            abs(dpx * pdx + dpy * pdy)
+
+
+def closest_segment(lines, pt):
+    c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
+    for i in range(1, len(lines)):
+        prev = lines[i - 1]
+        cur = lines[i]
+        c_pt_new, c_pt_new_dist = get_closest(prev, cur, pt)
+        if c_pt_new_dist < c_pt_dist:
+            c_pt = c_pt_new
+            c_pt_dist = c_pt_new_dist
+    return c_pt, c_pt_dist
+
+
+# Create a GTK+ widget on which we will draw using Cairo
+class Silly(basic_window.BaseWindow):
+
+    def __init__(self):
+        super(Silly, self).__init__()
+
+        self.window = Gtk.Window()
+        self.window.set_title("DrawingArea")
+
+        self.window.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)
+        self.method_connect("key-press-event", self.do_key_press)
+        self.method_connect("button-press-event",
+                            self._do_button_press_internal)
+        self.method_connect("configure-event", self._do_configure)
+        self.window.add(self)
+        self.window.show_all()
+
+        self.theta_version = False
+        self.reinit_extents()
+
+        self.last_pos = (numpy.pi / 2.0, 1.0)
+        self.circular_index_select = -1
+
+        # Extra stuff for drawing lines.
+        self.segments = []
+        self.prev_segment_pt = None
+        self.now_segment_pt = None
+        self.spline_edit = 0
+        self.edit_control1 = True
+
+    def do_key_press(self, event):
+        pass
+
+    def _do_button_press_internal(self, event):
+        o_x = event.x
+        o_y = event.y
+        x = event.x - self.window_shape[0] / 2
+        y = self.window_shape[1] / 2 - event.y
+        scale = self.get_current_scale()
+        event.x = x / scale + self.center[0]
+        event.y = y / scale + self.center[1]
+        self.do_button_press(event)
+        event.x = o_x
+        event.y = o_y
+
+    def do_button_press(self, event):
+        pass
+
+    def _do_configure(self, event):
+        self.window_shape = (event.width, event.height)
+
+    def redraw(self):
+        if not self.needs_redraw:
+            self.needs_redraw = True
+            self.window.queue_draw()
+
+    def method_connect(self, event, cb):
+
+        def handler(obj, *args):
+            cb(*args)
+
+        self.window.connect(event, handler)
+
+    def reinit_extents(self):
+        if self.theta_version:
+            self.extents_x_min = -numpy.pi * 2
+            self.extents_x_max = numpy.pi * 2
+            self.extents_y_min = -numpy.pi * 2
+            self.extents_y_max = numpy.pi * 2
+        else:
+            self.extents_x_min = -40.0 * 0.0254
+            self.extents_x_max = 40.0 * 0.0254
+            self.extents_y_min = -4.0 * 0.0254
+            self.extents_y_max = 110.0 * 0.0254
+
+        self.init_extents(
+            (0.5 * (self.extents_x_min + self.extents_x_max), 0.5 *
+             (self.extents_y_max + self.extents_y_min)),
+            (1.0 * (self.extents_x_max - self.extents_x_min), 1.0 *
+             (self.extents_y_max - self.extents_y_min)))
+
+    # Handle the expose-event by drawing
+    def handle_draw(self, cr):
+        # use "with px(cr): blah;" to transform to pixel coordinates.
+
+        # Fill the background color of the window with grey
+        set_color(cr, palette["GREY"])
+        cr.paint()
+
+        # Draw a extents rectangle
+        set_color(cr, palette["WHITE"])
+        cr.rectangle(self.extents_x_min, self.extents_y_min,
+                     (self.extents_x_max - self.extents_x_min),
+                     self.extents_y_max - self.extents_y_min)
+        cr.fill()
+
+        if not self.theta_version:
+            # Draw a filled white rectangle.
+            set_color(cr, palette["WHITE"])
+            cr.rectangle(-2.0, -2.0, 4.0, 4.0)
+            cr.fill()
+
+            set_color(cr, palette["BLUE"])
+            cr.arc(joint_center[0], joint_center[1], l2 + l1, 0,
+                   2.0 * numpy.pi)
+            with px(cr):
+                cr.stroke()
+            cr.arc(joint_center[0], joint_center[1], l1 - l2, 0,
+                   2.0 * numpy.pi)
+            with px(cr):
+                cr.stroke()
+        else:
+            # Draw a filled white rectangle.
+            set_color(cr, palette["WHITE"])
+            cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
+            cr.fill()
+
+        if self.theta_version:
+            set_color(cr, palette["BLUE"])
+            for i in range(-6, 6):
+                cr.move_to(-40, -40 + i * numpy.pi)
+                cr.line_to(40, 40 + i * numpy.pi)
+            with px(cr):
+                cr.stroke()
+
+        if self.theta_version:
+            set_color(cr, Color(0.5, 0.5, 1.0))
+            draw_lines(cr, lines_theta)
+        else:
+            set_color(cr, Color(0.5, 1.0, 1.0))
+            draw_lines(cr, lines1)
+            draw_lines(cr, lines2)
+
+            def get_circular_index(pt):
+                theta1, theta2 = pt
+                circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+                return circular_index
+
+            set_color(cr, palette["BLUE"])
+            lines = subdivide_theta(lines_theta)
+            o_circular_index = circular_index = get_circular_index(lines[0])
+            p_xy = to_xy(lines[0][0], lines[0][1])
+            if circular_index == self.circular_index_select:
+                cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
+            for pt in lines[1:]:
+                p_xy = to_xy(pt[0], pt[1])
+                circular_index = get_circular_index(pt)
+                if o_circular_index == self.circular_index_select:
+                    cr.line_to(p_xy[0] + o_circular_index * 0, p_xy[1])
+                if circular_index != o_circular_index:
+                    o_circular_index = circular_index
+                    with px(cr):
+                        cr.stroke()
+                    if circular_index == self.circular_index_select:
+                        cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
+
+            with px(cr):
+                cr.stroke()
+
+        if not self.theta_version:
+            theta1, theta2 = to_theta(self.last_pos,
+                                      self.circular_index_select)
+            x, y = joint_center[0], joint_center[1]
+            cr.move_to(x, y)
+
+            x += numpy.cos(theta1) * l1
+            y += numpy.sin(theta1) * l1
+            cr.line_to(x, y)
+            x += numpy.cos(theta2) * l2
+            y += numpy.sin(theta2) * l2
+            cr.line_to(x, y)
+            with px(cr):
+                cr.stroke()
+
+            cr.move_to(self.last_pos[0], self.last_pos[1])
+            set_color(cr, Color(0.0, 1.0, 0.2))
+            draw_px_cross(cr, 20)
+
+        if self.theta_version:
+            set_color(cr, Color(0.0, 1.0, 0.2))
+            cr.move_to(self.last_pos[0], self.last_pos[1])
+            draw_px_cross(cr, 5)
+
+            c_pt, dist = closest_segment(lines_theta, self.last_pos)
+            print("dist:", dist, c_pt, self.last_pos)
+            set_color(cr, palette["CYAN"])
+            cr.move_to(c_pt[0], c_pt[1])
+            draw_px_cross(cr, 5)
+
+        set_color(cr, Color(0.0, 0.5, 1.0))
+        for segment in self.segments:
+            color = [0, random.random(), 1]
+            random.shuffle(color)
+            set_color(cr, Color(color[0], color[1], color[2]))
+            segment.DrawTo(cr, self.theta_version)
+            with px(cr):
+                cr.stroke()
+
+        set_color(cr, Color(0.0, 1.0, 0.5))
+        segment = self.current_seg()
+        if segment:
+            print(segment)
+            segment.DrawTo(cr, self.theta_version)
+            with px(cr):
+                cr.stroke()
+
+    def cur_pt_in_theta(self):
+        if self.theta_version: return self.last_pos
+        return to_theta(self.last_pos, self.circular_index_select)
+
+    # Current segment based on which mode the drawing system is in.
+    def current_seg(self):
+        if self.prev_segment_pt and self.now_segment_pt:
+            if self.theta_version:
+                return AngleSegment(self.prev_segment_pt, self.now_segment_pt)
+            else:
+                return XYSegment(self.prev_segment_pt, self.now_segment_pt)
+
+    def do_key_press(self, event):
+        keyval = Gdk.keyval_to_lower(event.keyval)
+        print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+        if keyval == Gdk.KEY_q:
+            print("Found q key and exiting.")
+            quit_main_loop()
+        elif keyval == Gdk.KEY_c:
+            # Increment which arm solution we render
+            self.circular_index_select += 1
+            print(self.circular_index_select)
+        elif keyval == Gdk.KEY_v:
+            # Decrement which arm solution we render
+            self.circular_index_select -= 1
+            print(self.circular_index_select)
+        elif keyval == Gdk.KEY_w:
+            # Add this segment to the segment list.
+            segment = self.current_seg()
+            if segment: self.segments.append(segment)
+            self.prev_segment_pt = self.now_segment_pt
+
+        elif keyval == Gdk.KEY_r:
+            self.prev_segment_pt = self.now_segment_pt
+
+        elif keyval == Gdk.KEY_p:
+            # Print out the segments.
+            print(repr(self.segments))
+        elif keyval == Gdk.KEY_g:
+            # Generate theta points.
+            if self.segments:
+                print(repr(self.segments[0].ToThetaPoints()))
+        elif keyval == Gdk.KEY_e:
+            best_pt = self.now_segment_pt
+            best_dist = 1e10
+            for segment in self.segments:
+                d = angle_dist_sqr(segment.start, self.now_segment_pt)
+                if (d < best_dist):
+                    best_pt = segment.start
+                    best_dist = d
+                d = angle_dist_sqr(segment.end, self.now_segment_pt)
+                if (d < best_dist):
+                    best_pt = segment.end
+                    best_dist = d
+            self.now_segment_pt = best_pt
+
+        elif keyval == Gdk.KEY_t:
+            # Toggle between theta and xy renderings
+            if self.theta_version:
+                theta1, theta2 = self.last_pos
+                data = to_xy(theta1, theta2)
+                self.circular_index_select = int(
+                    numpy.floor((theta2 - theta1) / numpy.pi))
+                self.last_pos = (data[0], data[1])
+            else:
+                self.last_pos = self.cur_pt_in_theta()
+
+            self.theta_version = not self.theta_version
+            self.reinit_extents()
+
+        elif keyval == Gdk.KEY_z:
+            self.edit_control1 = not self.edit_control1
+            if self.edit_control1:
+                self.now_segment_pt = self.segments[0].control1
+            else:
+                self.now_segment_pt = self.segments[0].control2
+            if not self.theta_version:
+                data = to_xy(self.now_segment_pt[0], self.now_segment_pt[1])
+                self.last_pos = (data[0], data[1])
+            else:
+                self.last_pos = self.now_segment_pt
+
+            print("self.last_pos: ", self.last_pos, " ci: ",
+                  self.circular_index_select)
+
+        self.redraw()
+
+    def do_button_press(self, event):
+        self.last_pos = (event.x, event.y)
+        self.now_segment_pt = self.cur_pt_in_theta()
+
+        if self.edit_control1:
+            self.segments[0].control1 = self.now_segment_pt
+        else:
+            self.segments[0].control2 = self.now_segment_pt
+
+        print('Clicked at theta: %s' % (repr(self.now_segment_pt, )))
+        if not self.theta_version:
+            print('Clicked at xy, circular index: (%f, %f, %f)' %
+                  (self.last_pos[0], self.last_pos[1],
+                   self.circular_index_select))
+
+        print('c1: numpy.array([%f, %f])' %
+              (self.segments[0].control1[0], self.segments[0].control1[1]))
+        print('c2: numpy.array([%f, %f])' %
+              (self.segments[0].control2[0], self.segments[0].control2[1]))
+
+        self.redraw()
+
+
+silly = Silly()
+silly.segments = graph_generate.segments
+basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_generate.py b/y2023/control_loops/python/graph_generate.py
new file mode 100644
index 0000000..046b9dd
--- /dev/null
+++ b/y2023/control_loops/python/graph_generate.py
@@ -0,0 +1,798 @@
+import numpy
+
+# joint_center in x-y space.
+joint_center = (-0.299, 0.299)
+
+# Joint distances (l1 = "proximal", l2 = "distal")
+l1 = 46.25 * 0.0254
+l2 = 43.75 * 0.0254
+
+
+# Convert from x-y coordinates to theta coordinates.
+# orientation is a bool. This orientation is circular_index mod 2.
+# where circular_index is the circular index, or the position in the
+# "hyperextension" zones. "cross_point" allows shifting the place where
+# it rounds the result so that it draws nicer (no other functional differences).
+def to_theta(pt, circular_index, cross_point=-numpy.pi):
+    orient = (circular_index % 2) == 0
+    x = pt[0]
+    y = pt[1]
+    x -= joint_center[0]
+    y -= joint_center[1]
+    l3 = numpy.hypot(x, y)
+    t3 = numpy.arctan2(y, x)
+    theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
+
+    if orient:
+        theta1 = -theta1
+    theta1 += t3
+    theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
+    theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
+                           x - l1 * numpy.cos(theta1))
+    return numpy.array((theta1, theta2))
+
+
+# Simple trig to go back from theta1, theta2 to x-y
+def to_xy(theta1, theta2):
+    x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+    y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+    orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
+    return (x, y, orient)
+
+
+def get_circular_index(theta):
+    return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
+
+
+def get_xy(theta):
+    theta1 = theta[0]
+    theta2 = theta[1]
+    x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+    y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+    return numpy.array((x, y))
+
+
+# Draw a list of lines to a cairo context.
+def draw_lines(cr, lines):
+    cr.move_to(lines[0][0], lines[0][1])
+    for pt in lines[1:]:
+        cr.line_to(pt[0], pt[1])
+
+
+max_dist = 0.01
+max_dist_theta = numpy.pi / 64
+xy_end_circle_size = 0.01
+theta_end_circle_size = 0.07
+
+
+# Subdivide in theta space.
+def subdivide_theta(lines):
+    out = []
+    last_pt = lines[0]
+    out.append(last_pt)
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist_theta):
+            out.append(pt)
+        last_pt = n_pt
+
+    return out
+
+
+# subdivide in xy space.
+def subdivide_xy(lines, max_dist=max_dist):
+    out = []
+    last_pt = lines[0]
+    out.append(last_pt)
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist):
+            out.append(pt)
+        last_pt = n_pt
+
+    return out
+
+
+def to_theta_with_ci(pt, circular_index):
+    return to_theta_with_circular_index(pt[0], pt[1], circular_index)
+
+
+# to_theta, but distinguishes between
+def to_theta_with_circular_index(x, y, circular_index):
+    theta1, theta2 = to_theta((x, y), circular_index)
+    n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+    theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
+    return numpy.array((theta1, theta2))
+
+
+# alpha is in [0, 1] and is the weight to merge a and b.
+def alpha_blend(a, b, alpha):
+    """Blends a and b.
+
+    Args:
+      alpha: double, Ratio.  Needs to be in [0, 1] and is the weight to blend a
+          and b.
+    """
+    return b * alpha + (1.0 - alpha) * a
+
+
+def normalize(v):
+    """Normalize a vector while handling 0 length vectors."""
+    norm = numpy.linalg.norm(v)
+    if norm == 0:
+        return v
+    return v / norm
+
+
+# CI is circular index and allows selecting between all the stats that map
+# to the same x-y state (by giving them an integer index).
+# This will compute approximate first and second derivatives with respect
+# to path length.
+def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+                                            circular_index_select):
+    a = to_theta_with_circular_index(x, y, circular_index_select)
+    b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
+                                     circular_index_select)
+    c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
+                                     circular_index_select)
+    d1 = normalize(b - a)
+    d2 = normalize(c - a)
+    accel = (d1 + d2) / numpy.linalg.norm(a - b)
+    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
+    a = to_theta(p, c_i_select)
+    b = to_theta(p_next, c_i_select)
+    c = to_theta(p_prev, c_i_select)
+    d1 = normalize(b - a)
+    d2 = normalize(c - a)
+    accel = (d1 + d2) / numpy.linalg.norm(a - b)
+    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Generic subdivision algorithm.
+def subdivide(p1, p2, max_dist):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    dist = numpy.sqrt(dx**2 + dy**2)
+    n = int(numpy.ceil(dist / max_dist))
+    return [(alpha_blend(p1[0], p2[0],
+                         float(i) / n), alpha_blend(p1[1], p2[1],
+                                                    float(i) / n))
+            for i in range(1, n + 1)]
+
+
+# convert from an xy space loop into a theta loop.
+# All segements are expected go from one "hyper-extension" boundary
+# to another, thus we must go backwards over the "loop" to get a loop in
+# x-y space.
+def to_theta_loop(lines, cross_point=-numpy.pi):
+    out = []
+    last_pt = lines[0]
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist):
+            out.append(to_theta(pt, 0, cross_point))
+        last_pt = n_pt
+    for n_pt in reversed(lines[:-1]):
+        for pt in subdivide(last_pt, n_pt, max_dist):
+            out.append(to_theta(pt, 1, cross_point))
+        last_pt = n_pt
+    return out
+
+
+# Convert a loop (list of line segments) into
+# The name incorrectly suggests that it is cyclic.
+def back_to_xy_loop(lines):
+    out = []
+    last_pt = lines[0]
+    out.append(to_xy(last_pt[0], last_pt[1]))
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist_theta):
+            out.append(to_xy(pt[0], pt[1]))
+        last_pt = n_pt
+
+    return out
+
+
+# Segment in angle space.
+class AngleSegment:
+
+    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+        """Creates an angle segment.
+
+        Args:
+          start: (double, double),  The start of the segment in theta1, theta2
+              coordinates in radians
+          end: (double, double),  The end of the segment in theta1, theta2
+              coordinates in radians
+        """
+        self.start = start
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if theta_version:
+            cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
+            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
+            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.start[0], self.start[1])
+            cr.line_to(self.end[0], self.end[1])
+        else:
+            start_xy = to_xy(self.start[0], self.start[1])
+            end_xy = to_xy(self.end[0], self.end[1])
+            draw_lines(cr, back_to_xy_loop([self.start, self.end]))
+            cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
+            cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
+            cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        dx = self.end[0] - self.start[0]
+        dy = self.end[1] - self.start[1]
+        mag = numpy.hypot(dx, dy)
+        dx /= mag
+        dy /= mag
+
+        return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
+                (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
+
+
+class XYSegment:
+    """Straight line in XY space."""
+
+    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+        """Creates an XY segment.
+
+        Args:
+          start: (double, double),  The start of the segment in theta1, theta2
+              coordinates in radians
+          end: (double, double),  The end of the segment in theta1, theta2
+              coordinates in radians
+        """
+        self.start = start
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if theta_version:
+            theta1, theta2 = self.start
+            circular_index_select = int(
+                numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+            start = get_xy(self.start)
+            end = get_xy(self.end)
+
+            ln = [(start[0], start[1]), (end[0], end[1])]
+            draw_lines(cr, [
+                to_theta_with_circular_index(x, y, circular_index_select)
+                for x, y in subdivide_xy(ln)
+            ])
+            cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+        else:
+            start = get_xy(self.start)
+            end = get_xy(self.end)
+            cr.move_to(start[0], start[1])
+            cr.line_to(end[0], end[1])
+            cr.move_to(start[0] + xy_end_circle_size, start[1])
+            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+            cr.move_to(end[0] + xy_end_circle_size, end[1])
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
+        theta1, theta2 = self.start
+        circular_index_select = int(
+            numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+        start = get_xy(self.start)
+        end = get_xy(self.end)
+
+        ln = [(start[0], start[1]), (end[0], end[1])]
+
+        dx = end[0] - start[0]
+        dy = end[1] - start[1]
+        mag = numpy.hypot(dx, dy)
+        dx /= mag
+        dy /= mag
+
+        return [
+            to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+                                                    circular_index_select)
+            for x, y in subdivide_xy(ln, 0.01)
+        ]
+
+
+def spline_eval(start, control1, control2, end, alpha):
+    a = alpha_blend(start, control1, alpha)
+    b = alpha_blend(control1, control2, alpha)
+    c = alpha_blend(control2, end, alpha)
+    return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+                       alpha)
+
+
+def subdivide_spline(start, control1, control2, end):
+    # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
+    n = 100
+    for i in range(0, n + 1):
+        yield i / float(n)
+
+
+class SplineSegment:
+
+    def __init__(self,
+                 start,
+                 control1,
+                 control2,
+                 end,
+                 name=None,
+                 alpha_unitizer=None,
+                 vmax=None):
+        self.start = start
+        self.control1 = control1
+        self.control2 = control2
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "SplineSegment(%s, %s, %s, %s)" % (repr(
+            self.start), repr(self.control1), repr(
+                self.control2), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if theta_version:
+            c_i_select = get_circular_index(self.start)
+            start = get_xy(self.start)
+            control1 = get_xy(self.control1)
+            control2 = get_xy(self.control2)
+            end = get_xy(self.end)
+
+            draw_lines(cr, [
+                to_theta(spline_eval(start, control1, control2, end, alpha),
+                         c_i_select)
+                for alpha in subdivide_spline(start, control1, control2, end)
+            ])
+            cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+        else:
+            start = get_xy(self.start)
+            control1 = get_xy(self.control1)
+            control2 = get_xy(self.control2)
+            end = get_xy(self.end)
+
+            draw_lines(cr, [
+                spline_eval(start, control1, control2, end, alpha)
+                for alpha in subdivide_spline(start, control1, control2, end)
+            ])
+
+            cr.move_to(start[0] + xy_end_circle_size, start[1])
+            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+            cr.move_to(end[0] + xy_end_circle_size, end[1])
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        t1, t2 = self.start
+        c_i_select = get_circular_index(self.start)
+        start = get_xy(self.start)
+        control1 = get_xy(self.control1)
+        control2 = get_xy(self.control2)
+        end = get_xy(self.end)
+
+        return [
+            to_theta_with_ci_and_derivs(
+                spline_eval(start, control1, control2, end, alpha - 0.00001),
+                spline_eval(start, control1, control2, end, alpha),
+                spline_eval(start, control1, control2, end, alpha + 0.00001),
+                c_i_select)
+            for alpha in subdivide_spline(start, control1, control2, end)
+        ]
+
+
+def get_derivs(t_prev, t, t_next):
+    c, a, b = t_prev, t, t_next
+    d1 = normalize(b - a)
+    d2 = normalize(c - a)
+    accel = (d1 + d2) / numpy.linalg.norm(a - b)
+    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+class ThetaSplineSegment:
+
+    def __init__(self,
+                 start,
+                 control1,
+                 control2,
+                 end,
+                 name=None,
+                 alpha_unitizer=None,
+                 vmax=None):
+        self.start = start
+        self.control1 = control1
+        self.control2 = control2
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+            self.start), repr(self.control1), repr(
+                self.control2), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if (theta_version):
+            draw_lines(cr, [
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha)
+                for alpha in subdivide_spline(self.start, self.control1,
+                                              self.control2, self.end)
+            ])
+        else:
+            start = get_xy(self.start)
+            end = get_xy(self.end)
+
+            draw_lines(cr, [
+                get_xy(
+                    spline_eval(self.start, self.control1, self.control2,
+                                self.end, alpha))
+                for alpha in subdivide_spline(self.start, self.control1,
+                                              self.control2, self.end)
+            ])
+
+            cr.move_to(start[0] + xy_end_circle_size, start[1])
+            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+            cr.move_to(end[0] + xy_end_circle_size, end[1])
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        return [
+            get_derivs(
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha - 0.00001),
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha),
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha + 0.00001))
+            for alpha in subdivide_spline(self.start, self.control1,
+                                          self.control2, self.end)
+        ]
+
+
+tall_box_x = 0.411
+tall_box_y = 0.125
+
+short_box_x = 0.431
+short_box_y = 0.082
+
+ready_above_box = to_theta_with_circular_index(tall_box_x,
+                                               tall_box_y + 0.08,
+                                               circular_index=-1)
+tall_box_grab = to_theta_with_circular_index(tall_box_x,
+                                             tall_box_y,
+                                             circular_index=-1)
+short_box_grab = to_theta_with_circular_index(short_box_x,
+                                              short_box_y,
+                                              circular_index=-1)
+
+# TODO(austin): Drive the front/back off the same numbers a bit better.
+front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(0.700,
+                                                 2.125,
+                                                 circular_index=-1.000000)
+front_middle2_box = to_theta_with_circular_index(0.700,
+                                                 2.268,
+                                                 circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(0.800,
+                                                 1.915,
+                                                 circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
+back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
+back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
+
+back_extra_low_box = to_theta_with_circular_index(-0.87,
+                                                  1.52,
+                                                  circular_index=0)
+
+front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
+back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
+
+neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
+
+up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+
+front_switch_auto = to_theta_with_circular_index(0.750,
+                                                 2.20,
+                                                 circular_index=-1.000000)
+
+duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
+
+starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
+
+self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
+
+above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
+below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
+
+up_c1 = to_theta((0.63, 1.17), circular_index=-1)
+up_c2 = to_theta((0.65, 1.62), circular_index=-1)
+
+front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
+front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
+
+front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
+front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
+
+front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
+front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
+
+#c1: (1.421433, -1.070254)
+#c2: (1.434384, -1.057803
+ready_above_box_c1 = numpy.array([1.480802, -1.081218])
+ready_above_box_c2 = numpy.array([1.391449, -1.060331])
+
+front_switch_c1 = numpy.array([1.903841, -0.622351])
+front_switch_c2 = numpy.array([1.903841, -0.622351])
+
+
+sparse_front_points = [
+    (front_high_box, "FrontHighBox"),
+    (front_middle2_box, "FrontMiddle2Box"),
+    (front_middle3_box, "FrontMiddle3Box"),
+    (front_middle1_box, "FrontMiddle1Box"),
+    (front_low_box, "FrontLowBox"),
+    (front_switch, "FrontSwitch"),
+]  # yapf: disable
+
+sparse_back_points = [
+    (back_high_box, "BackHighBox"),
+    (back_middle2_box, "BackMiddle2Box"),
+    (back_middle1_box, "BackMiddle1Box"),
+    (back_low_box, "BackLowBox"),
+    (back_extra_low_box, "BackExtraLowBox"),
+]  # yapf: disable
+
+def expand_points(points, max_distance):
+    """Expands a list of points to be at most max_distance apart
+
+    Generates the paths to connect the new points to the closest input points,
+    and the paths connecting the points.
+
+    Args:
+      points, list of tuple of point, name, The points to start with and fill
+          in.
+      max_distance, float, The max distance between two points when expanding
+          the graph.
+
+    Return:
+      points, edges
+    """
+    result_points = [points[0]]
+    result_paths = []
+    for point, name in points[1:]:
+        previous_point = result_points[-1][0]
+        previous_point_xy = get_xy(previous_point)
+        circular_index = get_circular_index(previous_point)
+
+        point_xy = get_xy(point)
+        norm = numpy.linalg.norm(point_xy - previous_point_xy)
+        num_points = int(numpy.ceil(norm / max_distance))
+        last_iteration_point = previous_point
+        for subindex in range(1, num_points):
+            subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
+                                            float(subindex) / num_points),
+                                circular_index=circular_index)
+            result_points.append(
+                (subpoint, '%s%dof%d' % (name, subindex, num_points)))
+            result_paths.append(
+                XYSegment(last_iteration_point, subpoint, vmax=6.0))
+            if (last_iteration_point != previous_point).any():
+                result_paths.append(XYSegment(previous_point, subpoint))
+            if subindex == num_points - 1:
+                result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+            else:
+                result_paths.append(XYSegment(subpoint, point))
+            last_iteration_point = subpoint
+        result_points.append((point, name))
+
+    return result_points, result_paths
+
+
+front_points, front_paths = expand_points(sparse_front_points, 0.06)
+back_points, back_paths = expand_points(sparse_back_points, 0.06)
+
+points = [(ready_above_box, "ReadyAboveBox"),
+          (tall_box_grab, "TallBoxGrab"),
+          (short_box_grab, "ShortBoxGrab"),
+          (back_switch, "BackSwitch"),
+          (neutral, "Neutral"),
+          (up, "Up"),
+          (above_hang, "AboveHang"),
+          (below_hang, "BelowHang"),
+          (self_hang, "SelfHang"),
+          (partner_hang, "PartnerHang"),
+          (front_switch_auto, "FrontSwitchAuto"),
+          (starting, "Starting"),
+          (duck, "Duck"),
+          (vertical_starting, "VerticalStarting"),
+] + front_points + back_points  # yapf: disable
+
+duck_c1 = numpy.array([1.337111, -1.721008])
+duck_c2 = numpy.array([1.283701, -1.795519])
+
+ready_to_up_c1 = numpy.array([1.792962, 0.198329])
+ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+
+front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
+front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
+
+# We need to define critical points so we can create paths connecting them.
+# TODO(austin): Attach velocities to the slow ones.
+ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
+
+neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
+
+tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
+tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
+
+tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
+
+# If true, only plot the first named segment
+isolate = False
+
+long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
+
+neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
+neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
+
+named_segments = [
+    ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
+                       back_switch, "BackSwitch"),
+    ThetaSplineSegment(neutral,
+                       neutral_to_back_low_c1,
+                       tall_to_back_high_c2,
+                       back_high_box,
+                       "NeutralBoxToHigh",
+                       alpha_unitizer=long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
+                       back_middle2_box, "NeutralBoxToMiddle2",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+                       back_middle1_box, "NeutralBoxToMiddle1",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+                       back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "ReadyBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_low_c2, back_middle1_box,
+                       "ReadyBoxToMiddle1", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "ShortBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_low_c2, back_middle1_box,
+                       "ShortBoxToMiddle1", long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "TallBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+                       back_middle1_box, "TallBoxToMiddle1",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+                       back_low_box, "TallBoxToLow", long_alpha_unitizer),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  ready_above_box, "ReadyToNeutral"),
+    XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
+    XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
+    XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  tall_box_grab, "TallToNeutral"),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  short_box_grab, "ShortToNeutral"),
+    SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
+    SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
+                  front_high_box, "NeutralToFrontHigh"),
+    SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
+                  front_middle2_box, "NeutralToFrontMiddle2"),
+    SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
+                  front_middle1_box, "NeutralToFrontMiddle1"),
+]
+
+unnamed_segments = [
+    SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
+                  front_switch_auto),
+    SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+    SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
+    ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
+    SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+    XYSegment(ready_above_box, front_low_box),
+    XYSegment(ready_above_box, front_switch),
+    XYSegment(ready_above_box, front_middle1_box),
+    XYSegment(ready_above_box, front_middle2_box),
+    XYSegment(ready_above_box, front_middle3_box),
+    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
+                  front_high_box),
+    AngleSegment(starting, vertical_starting),
+    AngleSegment(vertical_starting, neutral),
+    XYSegment(neutral, front_low_box),
+    XYSegment(up, front_high_box),
+    XYSegment(up, front_middle2_box),
+    XYSegment(front_middle3_box, up),
+    XYSegment(front_middle3_box, front_high_box),
+    XYSegment(front_middle3_box, front_middle2_box),
+    XYSegment(front_middle3_box, front_middle1_box),
+    XYSegment(up, front_middle1_box),
+    XYSegment(up, front_low_box),
+    XYSegment(front_high_box, front_middle2_box),
+    XYSegment(front_high_box, front_middle1_box),
+    XYSegment(front_high_box, front_low_box),
+    XYSegment(front_middle2_box, front_middle1_box),
+    XYSegment(front_middle2_box, front_low_box),
+    XYSegment(front_middle1_box, front_low_box),
+    XYSegment(front_switch, front_low_box),
+    XYSegment(front_switch, up),
+    XYSegment(front_switch, front_high_box),
+    AngleSegment(up, back_high_box),
+    AngleSegment(up, back_middle2_box),
+    AngleSegment(up, back_middle1_box),
+    AngleSegment(up, back_low_box),
+    XYSegment(back_high_box, back_middle2_box),
+    XYSegment(back_high_box, back_middle1_box),
+    XYSegment(back_high_box, back_low_box),
+    XYSegment(back_middle2_box, back_middle1_box),
+    XYSegment(back_middle2_box, back_low_box),
+    XYSegment(back_middle1_box, back_low_box),
+    AngleSegment(up, above_hang),
+    AngleSegment(above_hang, below_hang),
+    AngleSegment(up, below_hang),
+    AngleSegment(up, self_hang),
+    AngleSegment(up, partner_hang),
+] + front_paths + back_paths
+
+segments = []
+if isolate:
+    segments += named_segments[:isolate]
+else:
+    segments += named_segments + unnamed_segments
diff --git a/y2023/rockpi/BUILD b/y2023/rockpi/BUILD
new file mode 100644
index 0000000..91e8729
--- /dev/null
+++ b/y2023/rockpi/BUILD
@@ -0,0 +1,11 @@
+cc_binary(
+    name = "imu_main",
+    srcs = ["imu_main.cc"],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/imu_reader:imu",
+    ],
+)
diff --git a/y2023/rockpi/imu_main.cc b/y2023/rockpi/imu_main.cc
new file mode 100644
index 0000000..ac0c141
--- /dev/null
+++ b/y2023/rockpi/imu_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+  aos::InitGoogle(&argc, &argv);
+
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+  // TODO(austin): Set the ratio...
+  frc971::imu::Imu imu(&event_loop, 1.0);
+
+  event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+  event_loop.SetRuntimeRealtimePriority(55);
+
+  event_loop.Run();
+
+  return 0;
+}
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 6ef0657..2ddb735 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -69,9 +69,11 @@
     visibility = ["//y2023:__subpackages__"],
     deps = [
         "//aos:init",
+        "//aos:json_to_flatbuffer",
         "//aos/events:shm_event_loop",
         "//frc971/vision:vision_fbs",
         "//third_party:opencv",
+        "@com_google_absl//absl/strings",
     ],
 )
 
@@ -86,6 +88,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//y2023:__subpackages__"],
     deps = [
+        ":aprilrobotics_lib",
         ":calibration_data",
         "//aos:init",
         "//aos/events:simulated_event_loop",
@@ -97,3 +100,39 @@
         "//third_party:opencv",
     ],
 )
+
+cc_library(
+    name = "aprilrobotics_lib",
+    srcs = [
+        "aprilrobotics.cc",
+        "aprilrobotics.h",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2023:__subpackages__"],
+    deps = [
+        ":calibration_data",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+        "//frc971/vision:calibration_fbs",
+        "//frc971/vision:charuco_lib",
+        "//frc971/vision:target_map_fbs",
+        "//frc971/vision:target_mapper",
+        "//frc971/vision:vision_fbs",
+        "//third_party:opencv",
+        "//third_party/apriltag",
+    ],
+)
+
+cc_binary(
+    name = "aprilrobotics",
+    srcs = [
+        "aprilrobotics_main.cc",
+    ],
+    target_compatible_with = ["@platforms//os:linux"],
+    visibility = ["//y2023:__subpackages__"],
+    deps = [
+        ":aprilrobotics_lib",
+        "//aos:init",
+        "//aos/events:shm_event_loop",
+    ],
+)
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
new file mode 100644
index 0000000..d20a247
--- /dev/null
+++ b/y2023/vision/aprilrobotics.cc
@@ -0,0 +1,175 @@
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_bool(
+    debug, false,
+    "If true, dump a ton of debug and crash on the first valid detection.");
+
+DEFINE_int32(team_number, 971,
+             "Use the calibration for a node with this team number");
+namespace y2023 {
+namespace vision {
+
+AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
+                                             std::string_view channel_name)
+    : calibration_data_(CalibrationData()),
+      ftrace_(),
+      image_callback_(event_loop, channel_name,
+                      [&](cv::Mat image_color_mat,
+                          const aos::monotonic_clock::time_point eof) {
+                        HandleImage(image_color_mat, eof);
+                      }),
+      target_map_sender_(
+          event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+  tag_family_ = tag16h5_create();
+  tag_detector_ = apriltag_detector_create();
+
+  apriltag_detector_add_family_bits(tag_detector_, tag_family_, 1);
+  tag_detector_->nthreads = 6;
+  tag_detector_->wp = workerpool_create(tag_detector_->nthreads);
+  tag_detector_->qtp.min_white_black_diff = 5;
+  tag_detector_->debug = FLAGS_debug;
+
+  std::string hostname = aos::network::GetHostname();
+
+  // Check team string is valid
+  std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(hostname);
+  std::optional<uint16_t> team_number =
+      aos::network::team_number_internal::ParsePiTeamNumber(hostname);
+  CHECK(pi_number) << "Unable to parse pi number from '" << hostname << "'";
+  CHECK(team_number);
+
+  calibration_ = FindCameraCalibration(&calibration_data_.message(),
+                                       "pi" + std::to_string(*pi_number));
+  intrinsics_ = CameraIntrinsics(calibration_);
+  camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
+
+  image_callback_.set_format(frc971::vision::ImageCallback::Format::GRAYSCALE);
+}
+
+AprilRoboticsDetector::~AprilRoboticsDetector() {
+  apriltag_detector_destroy(tag_detector_);
+  free(tag_family_);
+}
+
+void AprilRoboticsDetector::SetWorkerpoolAffinities() {
+  for (int i = 0; i < tag_detector_->wp->nthreads; i++) {
+    cpu_set_t affinity;
+    CPU_ZERO(&affinity);
+    CPU_SET(i, &affinity);
+    pthread_setaffinity_np(tag_detector_->wp->threads[i], sizeof(affinity),
+                           &affinity);
+    struct sched_param param;
+    param.sched_priority = 20;
+    int res = pthread_setschedparam(tag_detector_->wp->threads[i], SCHED_FIFO,
+                                    &param);
+    PCHECK(res == 0) << "Failed to set priority of threadpool threads";
+  }
+}
+
+void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+                                        aos::monotonic_clock::time_point eof) {
+  std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
+      DetectTags(image_color_mat);
+
+  auto builder = target_map_sender_.MakeBuilder();
+  std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
+  for (const auto &[detection, pose] : detections) {
+    target_poses.emplace_back(
+        BuildTargetPose(pose, detection.id, builder.fbb()));
+  }
+  const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
+  auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
+  target_map_builder.add_target_poses(target_poses_offset);
+  target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
+  builder.CheckOk(builder.Send(target_map_builder.Finish()));
+}
+
+flatbuffers::Offset<frc971::vision::TargetPoseFbs>
+AprilRoboticsDetector::BuildTargetPose(
+    const apriltag_pose_t &pose,
+    frc971::vision::TargetMapper::TargetId target_id,
+    flatbuffers::FlatBufferBuilder *fbb) {
+  const auto T =
+      Eigen::Translation3d(pose.t->data[0], pose.t->data[1], pose.t->data[2]);
+  const auto position_offset =
+      frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
+
+  const auto orientation = Eigen::Quaterniond(Eigen::Matrix3d(pose.R->data));
+  const auto orientation_offset = frc971::vision::CreateQuaternion(
+      *fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());
+
+  return frc971::vision::CreateTargetPoseFbs(*fbb, target_id, position_offset,
+                                             orientation_offset);
+}
+
+std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
+AprilRoboticsDetector::DetectTags(cv::Mat image) {
+  const aos::monotonic_clock::time_point start_time =
+      aos::monotonic_clock::now();
+
+  image_u8_t im = {
+      .width = image.cols,
+      .height = image.rows,
+      .stride = image.cols,
+      .buf = image.data,
+  };
+
+  ftrace_.FormatMessage("Starting detect\n");
+  zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
+  ftrace_.FormatMessage("Done detecting\n");
+
+  std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+
+  for (int i = 0; i < zarray_size(detections); i++) {
+    apriltag_detection_t *det;
+    zarray_get(detections, i, &det);
+
+    if (det->decision_margin > 30) {
+      VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
+              << " margin: " << det->decision_margin;
+
+      const aos::monotonic_clock::time_point before_pose_estimation =
+          aos::monotonic_clock::now();
+      // First create an apriltag_detection_info_t struct using your known
+      // parameters.
+      apriltag_detection_info_t info;
+      info.det = det;
+      info.tagsize = 0.1524;
+      info.fx = intrinsics_.at<double>(0, 0);
+      info.fy = intrinsics_.at<double>(1, 1);
+      info.cx = intrinsics_.at<double>(0, 2);
+      info.cy = intrinsics_.at<double>(1, 2);
+
+      apriltag_pose_t pose;
+      double err = estimate_tag_pose(&info, &pose);
+
+      VLOG(1) << "err: " << err;
+
+      results.emplace_back(*det, pose);
+
+      const aos::monotonic_clock::time_point after_pose_estimation =
+          aos::monotonic_clock::now();
+
+      VLOG(1) << "Took "
+              << std::chrono::duration<double>(after_pose_estimation -
+                                               before_pose_estimation)
+                     .count()
+              << " seconds for pose estimation";
+    }
+  }
+
+  apriltag_detections_destroy(detections);
+
+  const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+
+  timeprofile_display(tag_detector_->tp);
+
+  VLOG(1) << "Took "
+          << std::chrono::duration<double>(end_time - start_time).count()
+          << " seconds to detect overall";
+
+  return results;
+}
+
+}  // namespace vision
+}  // namespace y2023
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
new file mode 100644
index 0000000..a68b1d9
--- /dev/null
+++ b/y2023/vision/aprilrobotics.h
@@ -0,0 +1,101 @@
+
+#include <string>
+
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/network/team_number.h"
+#include "aos/realtime.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_generated.h"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/imgproc.hpp"
+#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/apriltag_pose.h"
+#include "third_party/apriltag/tag16h5.h"
+#include "y2023/vision/calibration_data.h"
+
+DECLARE_int32(team_number);
+
+namespace y2023 {
+namespace vision {
+
+class AprilRoboticsDetector {
+ public:
+  AprilRoboticsDetector(aos::EventLoop *event_loop,
+                        std::string_view channel_name);
+
+  ~AprilRoboticsDetector();
+
+  void SetWorkerpoolAffinities();
+
+  std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
+      cv::Mat image);
+
+ private:
+  void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
+
+  flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
+      const apriltag_pose_t &pose,
+      frc971::vision::TargetMapper::TargetId target_id,
+      flatbuffers::FlatBufferBuilder *fbb);
+
+  static const frc971::vision::calibration::CameraCalibration *
+  FindCameraCalibration(
+      const frc971::vision::calibration::CalibrationData *calibration_data,
+      std::string_view node_name) {
+    for (const frc971::vision::calibration::CameraCalibration *candidate :
+         *calibration_data->camera_calibrations()) {
+      if (candidate->node_name()->string_view() != node_name) {
+        continue;
+      }
+      if (candidate->team_number() != FLAGS_team_number) {
+        continue;
+      }
+      return candidate;
+    }
+    LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+               << " on " << FLAGS_team_number;
+  }
+
+  static cv::Mat CameraIntrinsics(
+      const frc971::vision::calibration::CameraCalibration
+          *camera_calibration) {
+    cv::Mat result(3, 3, CV_32F,
+                   const_cast<void *>(static_cast<const void *>(
+                       camera_calibration->intrinsics()->data())));
+    result.convertTo(result, CV_64F);
+    CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+
+    return result;
+  }
+
+  static cv::Mat CameraDistCoeffs(
+      const frc971::vision::calibration::CameraCalibration
+          *camera_calibration) {
+    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;
+  }
+
+  apriltag_family_t *tag_family_;
+  apriltag_detector_t *tag_detector_;
+
+  const aos::FlatbufferSpan<frc971::vision::calibration::CalibrationData>
+      calibration_data_;
+  const frc971::vision::calibration::CameraCalibration *calibration_;
+  cv::Mat intrinsics_;
+  cv::Mat camera_distortion_coeffs_;
+
+  aos::Ftrace ftrace_;
+
+  frc971::vision::ImageCallback image_callback_;
+  aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+};
+
+}  // namespace vision
+}  // namespace y2023
diff --git a/y2023/vision/aprilrobotics_main.cc b/y2023/vision/aprilrobotics_main.cc
new file mode 100644
index 0000000..67b853f
--- /dev/null
+++ b/y2023/vision/aprilrobotics_main.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace y2023::vision {
+void AprilViewerMain() {
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  AprilRoboticsDetector detector(&event_loop, "/camera");
+
+  detector.SetWorkerpoolAffinities();
+
+  event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+  struct sched_param param;
+  param.sched_priority = 21;
+  PCHECK(sched_setscheduler(0, SCHED_FIFO, &param) == 0);
+
+  event_loop.Run();
+}
+
+}  // namespace y2023::vision
+
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  y2023::vision::AprilViewerMain();
+
+  return 0;
+}
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
index a69caf3..b604516 100644
--- a/y2023/vision/camera_reader.cc
+++ b/y2023/vision/camera_reader.cc
@@ -16,6 +16,7 @@
 DEFINE_double(red, 1.252, "Red gain");
 DEFINE_double(green, 1, "Green gain");
 DEFINE_double(blue, 1.96, "Blue gain");
+DEFINE_double(exposure, 150, "Camera exposure");
 
 namespace y2023 {
 namespace vision {
@@ -62,8 +63,8 @@
       media_device->FindEntity("rkisp1_resizer_selfpath");
   rkisp1_resizer_selfpath->pads(0)->SetSubdevFormat(width, height,
                                                     MEDIA_BUS_FMT_YUYV8_2X8);
-  rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(width, height,
-                                                    MEDIA_BUS_FMT_YUYV8_2X8);
+  rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(
+      width * 2 / 3, height * 2 / 3, MEDIA_BUS_FMT_YUYV8_2X8);
   rkisp1_resizer_selfpath->pads(0)->SetSubdevCrop(width, height);
 
   Entity *rkisp1_resizer_mainpath =
@@ -79,7 +80,7 @@
   rkisp1_mainpath->SetFormat(width / 2, height / 2, V4L2_PIX_FMT_YUV422P);
 
   Entity *rkisp1_selfpath = media_device->FindEntity("rkisp1_selfpath");
-  rkisp1_selfpath->SetFormat(width, height, V4L2_PIX_FMT_YUYV);
+  rkisp1_selfpath->SetFormat(width * 2 / 3, height * 2 / 3, V4L2_PIX_FMT_YUYV);
 
   media_device->Enable(
       media_device->FindLink(camera_device_string, 0, "rkisp1_csi", 0));
@@ -104,7 +105,7 @@
   if (FLAGS_lowlight_camera) {
     v4l2_reader.SetGainExt(100);
     v4l2_reader.SetVerticalBlanking(1000);
-    v4l2_reader.SetExposure(150);
+    v4l2_reader.SetExposure(FLAGS_exposure);
   } else {
     v4l2_reader.SetGainExt(1000);
     v4l2_reader.SetExposure(1000);
diff --git a/y2023/vision/target_map.json b/y2023/vision/target_map.json
index 5f256fa..3f6eb54 100644
--- a/y2023/vision/target_map.json
+++ b/y2023/vision/target_map.json
@@ -2,75 +2,123 @@
     "target_poses": [
         {
             "id": 1,
-            "x": 7.244,
-            "y": -2.938,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.244,
+                "y": -2.938,
+                "z": 0.463
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 2,
-            "x": 7.244,
-            "y": -1.262,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.244,
+                "y": -1.262,
+                "z": 0.463
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 3,
-            "x": 7.244,
-            "y": 0.414,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.244,
+                "y": 0.414,
+                "z": 0.463
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 4,
-            "x": 7.909,
-            "y": 2.740,
-            "z": 0.695,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 3.141592653589793
+            "position": {
+                "x": 7.909,
+                "y": 2.740,
+                "z": 0.695
+            },
+            /* yaw of pi */
+            "orientation": {
+                "w": 0.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 1.0
+            }
         },
         {
             "id": 5,
-            "x": -7.908,
-            "y": 2.740,
-            "z": 0.695,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.908,
+                "y": 2.740,
+                "z": 0.695
+            },
+            "orientation": {
+            /* yaw of 0 */
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         },
         {
             "id": 6,
-            "x": -7.243,
-            "y": 0.414,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.243,
+                "y": 0.414,
+                "z": 0.463
+            },
+            /* yaw of 0 */
+            "orientation": {
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         },
         {
             "id": 7,
-            "x": -7.243,
-            "y": -1.262,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.243,
+                "y": -1.262,
+                "z": 0.463
+            },
+            /* yaw of 0 */
+            "orientation": {
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         },
         {
             "id": 8,
-            "x": -7.243,
-            "y": -2.938,
-            "z": 0.463,
-            "roll": 0.0,
-            "pitch": 0.0,
-            "yaw": 0.0
+            "position": {
+                "x": -7.243,
+                "y": -2.938,
+                "z": 0.463
+            },
+            /* yaw of 0 */
+            "orientation": {
+                "w": 1.0,
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            }
         }
     ]
 }
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index d4c8c50..6d32dbd 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -12,14 +12,12 @@
 #include "opencv2/highgui.hpp"
 #include "opencv2/highgui/highgui.hpp"
 #include "opencv2/imgproc.hpp"
+#include "y2023/vision/aprilrobotics.h"
 #include "y2023/vision/calibration_data.h"
 
 DEFINE_string(json_path, "target_map.json",
               "Specify path for json with initial pose guesses.");
-DEFINE_int32(team_number, 7971,
-             "Use the calibration for a node with this team number");
-
-DECLARE_string(image_channel);
+DECLARE_int32(team_number);
 
 namespace y2023 {
 namespace vision {
@@ -27,6 +25,7 @@
 using frc971::vision::DataAdapter;
 using frc971::vision::ImageCallback;
 using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
 using frc971::vision::TargetMapper;
 namespace calibration = frc971::vision::calibration;
 
@@ -40,21 +39,17 @@
 
 // Add detected apriltag poses relative to the robot to
 // timestamped_target_detections
-void HandleAprilTag(aos::distributed_clock::time_point pi_distributed_time,
-                    std::vector<cv::Vec4i> april_ids,
-                    std::vector<Eigen::Vector3d> rvecs_eigen,
-                    std::vector<Eigen::Vector3d> tvecs_eigen,
+void HandleAprilTag(const TargetMap &map,
+                    aos::distributed_clock::time_point pi_distributed_time,
                     std::vector<DataAdapter::TimestampedDetection>
                         *timestamped_target_detections,
                     Eigen::Affine3d extrinsics) {
-  for (size_t tag = 0; tag < april_ids.size(); tag++) {
-    Eigen::Translation3d T_camera_target = Eigen::Translation3d(
-        tvecs_eigen[tag][0], tvecs_eigen[tag][1], tvecs_eigen[tag][2]);
-    Eigen::AngleAxisd r_angle = Eigen::AngleAxisd(
-        rvecs_eigen[tag].norm(), rvecs_eigen[tag] / rvecs_eigen[tag].norm());
-    CHECK(rvecs_eigen[tag].norm() != 0) << "rvecs norm = 0; divide by 0";
+  for (const auto *target_pose_fbs : *map.target_poses()) {
+    const TargetMapper::TargetPose target_pose =
+        PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
 
-    Eigen::Affine3d H_camcv_target = T_camera_target * r_angle;
+    Eigen::Affine3d H_camcv_target =
+        Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
     // With X, Y, Z being robot axes and x, y, z being camera axes,
     // x = -Y, y = -Z, z = X
     static const Eigen::Affine3d H_camcv_camrob =
@@ -69,12 +64,15 @@
         PoseUtils::Affine3dToPose3d(H_camrob_target);
     double distance_from_camera = target_pose_camera.p.norm();
 
+    CHECK(map.has_monotonic_timestamp_ns())
+        << "Need detection timestamps for mapping";
+
     timestamped_target_detections->emplace_back(
         DataAdapter::TimestampedDetection{
             .time = pi_distributed_time,
             .H_robot_target = H_robot_target,
             .distance_from_camera = distance_from_camera,
-            .id = april_ids[tag][0]});
+            .id = static_cast<TargetMapper::TargetId>(target_pose.id)});
   }
 }
 
@@ -115,49 +113,33 @@
 
 // Get images from pi and pass apriltag positions to HandleAprilTag()
 void HandlePiCaptures(
-    int pi_number, aos::EventLoop *pi_event_loop,
-    aos::logger::LogReader *reader,
+    aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
     std::vector<DataAdapter::TimestampedDetection>
         *timestamped_target_detections,
-    std::vector<std::unique_ptr<CharucoExtractor>> *charuco_extractors,
-    std::vector<std::unique_ptr<ImageCallback>> *image_callbacks) {
+    std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
   const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
       CalibrationData());
-  const calibration::CameraCalibration *calibration = FindCameraCalibration(
-      &calibration_data.message(), "pi" + std::to_string(pi_number));
+
+  const auto node_name = pi_event_loop->node()->name()->string_view();
+  const calibration::CameraCalibration *calibration =
+      FindCameraCalibration(&calibration_data.message(), node_name);
   const auto extrinsics = CameraExtrinsics(calibration);
 
   // TODO(milind): change to /camera once we log at full frequency
   static constexpr std::string_view kImageChannel = "/camera/decimated";
-  charuco_extractors->emplace_back(std::make_unique<CharucoExtractor>(
-      pi_event_loop,
-      "pi-" + std::to_string(FLAGS_team_number) + "-" +
-          std::to_string(pi_number),
-      frc971::vision::TargetType::kAprilTag, kImageChannel,
-      [=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
-          std::vector<cv::Vec4i> april_ids,
-          std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
-          std::vector<Eigen::Vector3d> rvecs_eigen,
-          std::vector<Eigen::Vector3d> tvecs_eigen) {
-        aos::distributed_clock::time_point pi_distributed_time =
-            reader->event_loop_factory()
-                ->GetNodeEventLoopFactory(pi_event_loop->node())
-                ->ToDistributedClock(eof);
+  detectors->emplace_back(
+      std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel));
 
-        if (valid) {
-          HandleAprilTag(pi_distributed_time, april_ids, rvecs_eigen,
-                         tvecs_eigen, timestamped_target_detections,
-                         extrinsics);
-        }
-      }));
+  pi_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
+    aos::distributed_clock::time_point pi_distributed_time =
+        reader->event_loop_factory()
+            ->GetNodeEventLoopFactory(pi_event_loop->node())
+            ->ToDistributedClock(aos::monotonic_clock::time_point(
+                aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
 
-  image_callbacks->emplace_back(std::make_unique<ImageCallback>(
-      pi_event_loop, kImageChannel,
-      [&, charuco_extractor =
-              charuco_extractors->at(charuco_extractors->size() - 1).get()](
-          cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
-        charuco_extractor->HandleImage(rgb_image, eof);
-      }));
+    HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
+                   extrinsics);
+  });
 }
 
 void MappingMain(int argc, char *argv[]) {
@@ -170,40 +152,35 @@
   aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
   reader.Register();
 
-  std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
-  std::vector<std::unique_ptr<ImageCallback>> image_callbacks;
+  std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
 
   const aos::Node *pi1 =
       aos::configuration::GetNode(reader.configuration(), "pi1");
   std::unique_ptr<aos::EventLoop> pi1_event_loop =
       reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
-  HandlePiCaptures(1, pi1_event_loop.get(), &reader,
-                   &timestamped_target_detections, &charuco_extractors,
-                   &image_callbacks);
+  HandlePiCaptures(pi1_event_loop.get(), &reader,
+                   &timestamped_target_detections, &detectors);
 
   const aos::Node *pi2 =
       aos::configuration::GetNode(reader.configuration(), "pi2");
   std::unique_ptr<aos::EventLoop> pi2_event_loop =
       reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
-  HandlePiCaptures(2, pi2_event_loop.get(), &reader,
-                   &timestamped_target_detections, &charuco_extractors,
-                   &image_callbacks);
+  HandlePiCaptures(pi2_event_loop.get(), &reader,
+                   &timestamped_target_detections, &detectors);
 
   const aos::Node *pi3 =
       aos::configuration::GetNode(reader.configuration(), "pi3");
   std::unique_ptr<aos::EventLoop> pi3_event_loop =
       reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
-  HandlePiCaptures(3, pi3_event_loop.get(), &reader,
-                   &timestamped_target_detections, &charuco_extractors,
-                   &image_callbacks);
+  HandlePiCaptures(pi3_event_loop.get(), &reader,
+                   &timestamped_target_detections, &detectors);
 
   const aos::Node *pi4 =
       aos::configuration::GetNode(reader.configuration(), "pi4");
   std::unique_ptr<aos::EventLoop> pi4_event_loop =
       reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
-  HandlePiCaptures(4, pi4_event_loop.get(), &reader,
-                   &timestamped_target_detections, &charuco_extractors,
-                   &image_callbacks);
+  HandlePiCaptures(pi4_event_loop.get(), &reader,
+                   &timestamped_target_detections, &detectors);
 
   reader.event_loop_factory()->Run();
 
@@ -211,15 +188,11 @@
       DataAdapter::MatchTargetDetections(timestamped_target_detections);
 
   frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
-  mapper.Solve("rapid_react");
+  mapper.Solve("charged_up");
 
-  // Pointers need to be deleted to destruct all fetchers
-  for (auto &charuco_extractor_ptr : charuco_extractors) {
-    charuco_extractor_ptr.reset();
-  }
-
-  for (auto &image_callback_ptr : image_callbacks) {
-    image_callback_ptr.reset();
+  // Clean up all the pointers
+  for (auto &detector_ptr : detectors) {
+    detector_ptr.reset();
   }
 }
 
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 990ad7a..08def5b 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -1,8 +1,10 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc.hpp>
 
+#include "absl/strings/match.h"
 #include "aos/events/shm_event_loop.h"
 #include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
 #include "aos/time/time.h"
 #include "frc971/vision/vision_generated.h"
 
@@ -36,7 +38,12 @@
   cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
 
   if (!FLAGS_capture.empty()) {
-    cv::imwrite(FLAGS_capture, bgr_image);
+    if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
+      aos::WriteFlatbufferToFile(FLAGS_capture, image_fetcher.CopyFlatBuffer());
+    } else {
+      cv::imwrite(FLAGS_capture, bgr_image);
+    }
+
     return false;
   }
 
diff --git a/y2023/y2023.json b/y2023/y2023.json
index 76f0e52..d5f9462 100644
--- a/y2023/y2023.json
+++ b/y2023/y2023.json
@@ -1,5 +1,5 @@
 {
-  "channel_storage_duration": 2000000000,
+  "channel_storage_duration": 10000000000,
   "maps": [
     {
       "match": {
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index fd3e97f..fac37b2 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -164,12 +164,59 @@
       "type": "frc971.vision.CameraImage",
       "source_node": "pi{{ NUM }}",
       "frequency": 40,
-      "max_size": 4200000,
+      "max_size": 1843456,
       "num_readers": 4,
       "read_method": "PIN",
       "num_senders": 18
     },
     {
+      "name": "/pi{{ NUM }}/camera",
+      "type": "frc971.vision.TargetMap",
+      "source_node": "pi{{ NUM }}",
+      "frequency": 40,
+      "num_senders": 2,
+      "max_size": 40000,
+      "logger": "LOCAL_AND_REMOTE_LOGGER",
+      "logger_nodes": [
+        "imu",
+        "logger"
+      ],
+      "destination_nodes": [
+        {
+          "name": "imu",
+          "priority": 4,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "pi{{ NUM }}"
+          ],
+          "time_to_live": 5000000
+        },
+        {
+          "name": "logger",
+          "priority": 4,
+          "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+          "timestamp_logger_nodes": [
+            "pi{{ NUM }}"
+          ],
+          "time_to_live": 5000000
+        }
+      ]
+    },
+    {
+      "name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 80,
+      "source_node": "pi{{ NUM }}",
+      "max_size": 208
+    },
+    {
+      "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+      "type": "aos.message_bridge.RemoteMessage",
+      "frequency": 80,
+      "source_node": "pi{{ NUM }}",
+      "max_size": 208
+    },
+    {
       "name": "/logger/aos",
       "type": "aos.starter.StarterRpc",
       "source_node": "logger",