diff --git a/glass/.styleguide b/glass/.styleguide
index 5cd247583b..cf7a487147 100644
--- a/glass/.styleguide
+++ b/glass/.styleguide
@@ -21,6 +21,7 @@ repoRootNameOverride {
 includeOtherLibs {
   ^GLFW
   ^cscore
+  ^frc/
   ^imgui
   ^ntcore
   ^wpi/
diff --git a/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp
index dd73172101..a45c37c64f 100644
--- a/glass/src/lib/native/cpp/other/Field2D.cpp
+++ b/glass/src/lib/native/cpp/other/Field2D.cpp
@@ -4,12 +4,19 @@
 
 #include "glass/other/Field2D.h"
 
+#include <algorithm>
 #include <cmath>
 #include <memory>
+#include <utility>
+
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
 
 #define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_internal.h>
+#include <imgui_stdlib.h>
 #include <portable-file-dialogs.h>
 #include <units/angle.h>
 #include <units/length.h>
@@ -22,7 +29,6 @@
 #include <wpigui.h>
 
 #include "glass/Context.h"
-#include "glass/DataSource.h"
 
 using namespace glass;
 
@@ -30,50 +36,135 @@ namespace gui = wpi::gui;
 
 namespace {
 
+enum DisplayUnits { kDisplayMeters = 0, kDisplayFeet, kDisplayInches };
+
 // Per-frame field data (not persistent)
 struct FieldFrameData {
+  frc::Translation2d GetPosFromScreen(const ImVec2& cursor) const {
+    return {
+        units::meter_t{(std::clamp(cursor.x, min.x, max.x) - min.x) / scale},
+        units::meter_t{(max.y - std::clamp(cursor.y, min.y, max.y)) / scale}};
+  }
+  ImVec2 GetScreenFromPos(const frc::Translation2d& pos) const {
+    return {min.x + scale * pos.X().to<float>(),
+            max.y - scale * pos.Y().to<float>()};
+  }
+
   // in screen coordinates
   ImVec2 imageMin;
   ImVec2 imageMax;
   ImVec2 min;
   ImVec2 max;
 
-  float scale;  // scaling from field units to screen units
+  float scale;  // scaling from meters to screen units
 };
 
-// Object drag state
-struct ObjectDragState {
-  int object = 0;
-  int corner = 0;
+// Pose drag target info
+struct SelectedTargetInfo {
+  FieldObjectModel* objModel = nullptr;
+  std::string name;
+  size_t index;
+  units::radian_t rot;
+  ImVec2 poseCenter;  // center of the pose (screen coordinates)
+  ImVec2 center;      // center of the target (screen coordinates)
+  float radius;       // target radius
+  float dist;         // distance from center to mouse
+  int corner;         // corner (1 = center)
+};
+
+// Pose drag state
+struct PoseDragState {
+  SelectedTargetInfo target;
   ImVec2 initialOffset;
-  double initialAngle = 0;
+  units::radian_t initialAngle = 0_rad;
 };
 
-// Per-frame object data (not persistent)
-class ObjectFrameData {
+// Popup edit state
+class PopupState {
  public:
-  explicit ObjectFrameData(FieldObjectModel& model, const FieldFrameData& ffd,
-                           float width, float length);
-  void SetPosition(double x, double y);
-  // set and get rotation in radians
-  void SetRotation(double rot);
-  double GetRotation() const {
-    return units::convert<units::degrees, units::radians>(m_rot);
-  }
+  void Open(SelectedTargetInfo* target, const frc::Translation2d& pos);
+  void Close();
+
+  SelectedTargetInfo* GetTarget() { return &m_target; }
+  FieldObjectModel* GetInsertModel() { return m_insertModel; }
+  wpi::ArrayRef<frc::Pose2d> GetInsertPoses() const { return m_insertPoses; }
+
+  void Display(Field2DModel* model, const FieldFrameData& ffd);
+
+ private:
+  void DisplayTarget(Field2DModel* model, const FieldFrameData& ffd);
+  void DisplayInsert(Field2DModel* model);
+
+  SelectedTargetInfo m_target;
+
+  // for insert
+  FieldObjectModel* m_insertModel;
+  std::vector<frc::Pose2d> m_insertPoses;
+  std::string m_insertName;
+  int m_insertIndex;
+};
+
+struct DisplayOptions {
+  explicit DisplayOptions(const gui::Texture& texture) : texture{texture} {}
+
+  enum Style { kBoxImage = 0, kLine, kLineClosed, kTrack };
+
+  static constexpr Style kDefaultStyle = kBoxImage;
+  static constexpr float kDefaultWeight = 4.0f;
+  static constexpr ImU32 kDefaultColor = IM_COL32(255, 0, 0, 255);
+  static constexpr auto kDefaultWidth = 0.6858_m;
+  static constexpr auto kDefaultLength = 0.8204_m;
+  static constexpr bool kDefaultArrows = true;
+  static constexpr int kDefaultArrowSize = 50;
+  static constexpr float kDefaultArrowWeight = 4.0f;
+  static constexpr ImU32 kDefaultArrowColor = IM_COL32(0, 255, 0, 255);
+  static constexpr bool kDefaultSelectable = true;
+
+  Style style = kDefaultStyle;
+  float weight = kDefaultWeight;
+  int color = kDefaultColor;
+
+  units::meter_t width = kDefaultWidth;
+  units::meter_t length = kDefaultLength;
+
+  bool arrows = kDefaultArrows;
+  int arrowSize = kDefaultArrowSize;
+  float arrowWeight = kDefaultArrowWeight;
+  int arrowColor = kDefaultArrowColor;
+
+  bool selectable = kDefaultSelectable;
+
+  const gui::Texture& texture;
+};
+
+// Per-frame pose data (not persistent)
+class PoseFrameData {
+ public:
+  explicit PoseFrameData(const frc::Pose2d& pose, FieldObjectModel& model,
+                         size_t index, const FieldFrameData& ffd,
+                         const DisplayOptions& displayOptions);
+  void SetPosition(const frc::Translation2d& pos);
+  void SetRotation(units::radian_t rot);
+  const frc::Rotation2d& GetRotation() const { return m_pose.Rotation(); }
+  const frc::Pose2d& GetPose() const { return m_pose; }
+  float GetHitRadius() const { return m_hitRadius; }
   void UpdateFrameData();
-  int IsHovered(const ImVec2& cursor) const;
-  bool HandleDrag(const ImVec2& cursor, int hitCorner, ObjectDragState* drag);
-  void Draw(ImDrawList* drawList, const gui::Texture& texture,
-            int hitCorner) const;
+  std::pair<int, float> IsHovered(const ImVec2& cursor) const;
+  SelectedTargetInfo GetDragTarget(int corner, float dist) const;
+  void HandleDrag(const ImVec2& cursor);
+  void Draw(ImDrawList* drawList, std::vector<ImVec2>* center,
+            std::vector<ImVec2>* left, std::vector<ImVec2>* right) const;
 
   // in window coordinates
   ImVec2 m_center;
-  ImVec2 m_corners[4];
+  ImVec2 m_corners[6];  // 5 and 6 are used for track width
   ImVec2 m_arrow[3];
 
  private:
   FieldObjectModel& m_model;
+  size_t m_index;
   const FieldFrameData& m_ffd;
+  const DisplayOptions& m_displayOptions;
 
   // scaled width/2 and length/2, in screen units
   float m_width2;
@@ -81,58 +172,75 @@ class ObjectFrameData {
 
   float m_hitRadius;
 
-  double m_x = 0;
-  double m_y = 0;
-  double m_rot = 0;
+  frc::Pose2d m_pose;
 };
 
-class ObjectGroupInfo {
+class ObjectInfo {
  public:
-  static constexpr float kDefaultWidth = 0.6858f;
-  static constexpr float kDefaultLength = 0.8204f;
+  ObjectInfo();
 
-  ObjectGroupInfo();
+  DisplayOptions GetDisplayOptions() const;
+  void DisplaySettings();
+  void DrawLine(ImDrawList* drawList, wpi::ArrayRef<ImVec2> points) const;
 
-  std::unique_ptr<pfd::open_file> m_fileOpener;
-  float* m_pWidth;
-  float* m_pLength;
-  ObjectDragState m_dragState;
-
-  void Reset();
   void LoadImage();
   const gui::Texture& GetTexture() const { return m_texture; }
 
  private:
+  void Reset();
   bool LoadImageImpl(const char* fn);
 
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+
+  // in meters
+  float* m_pWidth;
+  float* m_pLength;
+
+  int* m_pStyle;  // DisplayOptions::Style
+  float* m_pWeight;
+  int* m_pColor;
+
+  bool* m_pArrows;
+  int* m_pArrowSize;
+  float* m_pArrowWeight;
+  int* m_pArrowColor;
+
+  bool* m_pSelectable;
+
   std::string* m_pFilename;
   gui::Texture m_texture;
 };
 
 class FieldInfo {
  public:
-  static constexpr float kDefaultWidth = 15.98f;
-  static constexpr float kDefaultHeight = 8.21f;
+  static constexpr auto kDefaultWidth = 15.98_m;
+  static constexpr auto kDefaultHeight = 8.21_m;
 
   FieldInfo();
 
-  std::unique_ptr<pfd::open_file> m_fileOpener;
-  float* m_pWidth;
-  float* m_pHeight;
+  void DisplaySettings();
 
-  void Reset();
   void LoadImage();
-  void LoadJson(const wpi::Twine& jsonfile);
   FieldFrameData GetFrameData(ImVec2 min, ImVec2 max) const;
   void Draw(ImDrawList* drawList, const FieldFrameData& frameData) const;
 
-  wpi::StringMap<std::unique_ptr<ObjectGroupInfo>> m_objectGroups;
+  wpi::StringMap<std::unique_ptr<ObjectInfo>> m_objects;
 
  private:
+  void Reset();
   bool LoadImageImpl(const char* fn);
+  void LoadJson(const wpi::Twine& jsonfile);
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
 
   std::string* m_pFilename;
   gui::Texture m_texture;
+
+  // in meters
+  float* m_pWidth;
+  float* m_pHeight;
+
+  // in image pixels
   int m_imageWidth;
   int m_imageHeight;
   int* m_pTop;
@@ -143,6 +251,86 @@ class FieldInfo {
 
 }  // namespace
 
+static PoseDragState gDragState;
+static PopupState gPopupState;
+static DisplayUnits gDisplayUnits = kDisplayMeters;
+
+static double ConvertDisplayLength(units::meter_t v) {
+  switch (gDisplayUnits) {
+    case kDisplayFeet:
+      return v.convert<units::feet>().to<double>();
+    case kDisplayInches:
+      return v.convert<units::inches>().to<double>();
+    case kDisplayMeters:
+    default:
+      return v.to<double>();
+  }
+}
+
+static double ConvertDisplayAngle(units::degree_t v) {
+  return v.to<double>();
+}
+
+static bool InputLength(const char* label, units::meter_t* v, double step = 0.0,
+                        double step_fast = 0.0, const char* format = "%.6f",
+                        ImGuiInputTextFlags flags = 0) {
+  double dv = ConvertDisplayLength(*v);
+  if (ImGui::InputDouble(label, &dv, step, step_fast, format, flags)) {
+    switch (gDisplayUnits) {
+      case kDisplayFeet:
+        *v = units::foot_t{dv};
+        break;
+      case kDisplayInches:
+        *v = units::inch_t{dv};
+        break;
+      case kDisplayMeters:
+      default:
+        *v = units::meter_t{dv};
+        break;
+    }
+    return true;
+  }
+  return false;
+}
+
+static bool InputFloatLength(const char* label, float* v, double step = 0.0,
+                             double step_fast = 0.0,
+                             const char* format = "%.3f",
+                             ImGuiInputTextFlags flags = 0) {
+  units::meter_t uv{*v};
+  if (InputLength(label, &uv, step, step_fast, format, flags)) {
+    *v = uv.to<float>();
+    return true;
+  }
+  return false;
+}
+
+static bool InputAngle(const char* label, units::degree_t* v, double step = 0.0,
+                       double step_fast = 0.0, const char* format = "%.6f",
+                       ImGuiInputTextFlags flags = 0) {
+  double dv = ConvertDisplayAngle(*v);
+  if (ImGui::InputDouble(label, &dv, step, step_fast, format, flags)) {
+    *v = units::degree_t{dv};
+    return true;
+  }
+  return false;
+}
+
+static bool InputPose(frc::Pose2d* pose) {
+  auto x = pose->X();
+  auto y = pose->Y();
+  auto rot = pose->Rotation().Degrees();
+
+  bool changed;
+  changed = InputLength("x", &x);
+  changed = InputLength("y", &y) || changed;
+  changed = InputAngle("rot", &rot) || changed;
+  if (changed) {
+    *pose = frc::Pose2d{x, y, rot};
+  }
+  return changed;
+}
+
 FieldInfo::FieldInfo() {
   auto& storage = GetStorage();
   m_pFilename = storage.GetStringRef("image");
@@ -150,8 +338,28 @@ FieldInfo::FieldInfo() {
   m_pLeft = storage.GetIntRef("left", 0);
   m_pBottom = storage.GetIntRef("bottom", -1);
   m_pRight = storage.GetIntRef("right", -1);
-  m_pWidth = storage.GetFloatRef("width", kDefaultWidth);
-  m_pHeight = storage.GetFloatRef("height", kDefaultHeight);
+  m_pWidth = storage.GetFloatRef("width", kDefaultWidth.to<float>());
+  m_pHeight = storage.GetFloatRef("height", kDefaultHeight.to<float>());
+}
+
+void FieldInfo::DisplaySettings() {
+  if (ImGui::Button("Choose image...")) {
+    m_fileOpener = std::make_unique<pfd::open_file>(
+        "Choose field image", "",
+        std::vector<std::string>{"Image File",
+                                 "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                                 "*.hdr *.pic *.ppm *.pgm",
+                                 "PathWeaver JSON File", "*.json"});
+  }
+  if (ImGui::Button("Reset image")) {
+    Reset();
+  }
+  InputFloatLength("Field Width", m_pWidth);
+  InputFloatLength("Field Height", m_pHeight);
+  // ImGui::InputInt("Field Top", m_pTop);
+  // ImGui::InputInt("Field Left", m_pLeft);
+  // ImGui::InputInt("Field Right", m_pRight);
+  // ImGui::InputInt("Field Bottom", m_pBottom);
 }
 
 void FieldInfo::Reset() {
@@ -330,19 +538,135 @@ void FieldInfo::Draw(ImDrawList* drawList, const FieldFrameData& ffd) const {
   drawList->AddRect(ffd.min, ffd.max, IM_COL32(255, 255, 0, 255));
 }
 
-ObjectGroupInfo::ObjectGroupInfo() {
+ObjectInfo::ObjectInfo() {
   auto& storage = GetStorage();
   m_pFilename = storage.GetStringRef("image");
-  m_pWidth = storage.GetFloatRef("width", kDefaultWidth);
-  m_pLength = storage.GetFloatRef("length", kDefaultLength);
+  m_pWidth =
+      storage.GetFloatRef("width", DisplayOptions::kDefaultWidth.to<float>());
+  m_pLength =
+      storage.GetFloatRef("length", DisplayOptions::kDefaultLength.to<float>());
+  m_pStyle = storage.GetIntRef("style", DisplayOptions::kDefaultStyle);
+  m_pWeight = storage.GetFloatRef("weight", DisplayOptions::kDefaultWeight);
+  m_pColor = storage.GetIntRef("color", DisplayOptions::kDefaultColor);
+  m_pArrows = storage.GetBoolRef("arrows", DisplayOptions::kDefaultArrows);
+  m_pArrowSize =
+      storage.GetIntRef("arrowSize", DisplayOptions::kDefaultArrowSize);
+  m_pArrowWeight =
+      storage.GetFloatRef("arrowWeight", DisplayOptions::kDefaultArrowWeight);
+  m_pArrowColor =
+      storage.GetIntRef("arrowColor", DisplayOptions::kDefaultArrowColor);
+  m_pSelectable =
+      storage.GetBoolRef("selectable", DisplayOptions::kDefaultSelectable);
 }
 
-void ObjectGroupInfo::Reset() {
+DisplayOptions ObjectInfo::GetDisplayOptions() const {
+  DisplayOptions rv{m_texture};
+  rv.style = static_cast<DisplayOptions::Style>(*m_pStyle);
+  rv.weight = *m_pWeight;
+  rv.color = *m_pColor;
+  rv.width = units::meter_t{*m_pWidth};
+  rv.length = units::meter_t{*m_pLength};
+  rv.arrows = *m_pArrows;
+  rv.arrowSize = *m_pArrowSize;
+  rv.arrowWeight = *m_pArrowWeight;
+  rv.arrowColor = *m_pArrowColor;
+  rv.selectable = *m_pSelectable;
+  return rv;
+}
+
+void ObjectInfo::DisplaySettings() {
+  static const char* styleChoices[] = {"Box/Image", "Line", "Line (Closed)",
+                                       "Track"};
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+  ImGui::Combo("Style", m_pStyle, styleChoices, IM_ARRAYSIZE(styleChoices));
+  switch (*m_pStyle) {
+    case DisplayOptions::kBoxImage:
+      if (ImGui::Button("Choose image...")) {
+        m_fileOpener = std::make_unique<pfd::open_file>(
+            "Choose object image", "",
+            std::vector<std::string>{
+                "Image File",
+                "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                "*.hdr *.pic *.ppm *.pgm"});
+      }
+      if (ImGui::Button("Reset image")) {
+        Reset();
+      }
+      InputFloatLength("Width", m_pWidth);
+      InputFloatLength("Length", m_pLength);
+      break;
+    case DisplayOptions::kTrack:
+      InputFloatLength("Width", m_pWidth);
+      break;
+    default:
+      break;
+  }
+
+  ImGui::InputFloat("Line Weight", m_pWeight);
+  ImColor col(*m_pColor);
+  if (ImGui::ColorEdit3("Line Color", &col.Value.x,
+                        ImGuiColorEditFlags_NoInputs)) {
+    *m_pColor = col;
+  }
+  ImGui::Checkbox("Arrows", m_pArrows);
+  if (*m_pArrows) {
+    ImGui::SliderInt("Arrow Size", m_pArrowSize, 0, 100, "%d%%",
+                     ImGuiSliderFlags_AlwaysClamp);
+    ImGui::InputFloat("Arrow Weight", m_pArrowWeight);
+    ImColor col(*m_pArrowColor);
+    if (ImGui::ColorEdit3("Arrow Color", &col.Value.x,
+                          ImGuiColorEditFlags_NoInputs)) {
+      *m_pArrowColor = col;
+    }
+  }
+
+  ImGui::Checkbox("Selectable", m_pSelectable);
+}
+
+void ObjectInfo::DrawLine(ImDrawList* drawList,
+                          wpi::ArrayRef<ImVec2> points) const {
+  if (points.empty()) {
+    return;
+  }
+
+  if (points.size() == 1) {
+    drawList->AddCircleFilled(points.front(), *m_pWeight, *m_pWeight);
+    return;
+  }
+
+  // PolyLine doesn't handle acute angles well; workaround from
+  // https://github.com/ocornut/imgui/issues/3366
+  size_t i = 0;
+  while (i + 1 < points.size()) {
+    int nlin = 2;
+    while (i + nlin < points.size()) {
+      auto [x0, y0] = points[i + nlin - 2];
+      auto [x1, y1] = points[i + nlin - 1];
+      auto [x2, y2] = points[i + nlin];
+      auto s0x = x1 - x0, s0y = y1 - y0;
+      auto s1x = x2 - x1, s1y = y2 - y1;
+      auto dotprod = s1x * s0x + s1y * s0y;
+      if (dotprod < 0) {
+        break;
+      }
+      ++nlin;
+    }
+
+    drawList->AddPolyline(&points[i], nlin, *m_pColor, false, *m_pWeight);
+    i += nlin - 1;
+  }
+
+  if (points.size() > 2 && *m_pStyle == DisplayOptions::kLineClosed) {
+    drawList->AddLine(points.back(), points.front(), *m_pColor, *m_pWeight);
+  }
+}
+
+void ObjectInfo::Reset() {
   m_texture = gui::Texture{};
   m_pFilename->clear();
 }
 
-void ObjectGroupInfo::LoadImage() {
+void ObjectInfo::LoadImage() {
   if (m_fileOpener && m_fileOpener->ready(0)) {
     auto result = m_fileOpener->result();
     if (!result.empty()) {
@@ -357,7 +681,7 @@ void ObjectGroupInfo::LoadImage() {
   }
 }
 
-bool ObjectGroupInfo::LoadImageImpl(const char* fn) {
+bool ObjectInfo::LoadImageImpl(const char* fn) {
   wpi::outs() << "GUI: loading object image '" << fn << "'\n";
   auto texture = gui::Texture::CreateFromFile(fn);
   if (!texture) {
@@ -369,144 +693,164 @@ bool ObjectGroupInfo::LoadImageImpl(const char* fn) {
   return true;
 }
 
-ObjectFrameData::ObjectFrameData(FieldObjectModel& model,
-                                 const FieldFrameData& ffd, float width,
-                                 float length)
+PoseFrameData::PoseFrameData(const frc::Pose2d& pose, FieldObjectModel& model,
+                             size_t index, const FieldFrameData& ffd,
+                             const DisplayOptions& displayOptions)
     : m_model{model},
+      m_index{index},
       m_ffd{ffd},
-      m_width2(ffd.scale * width / 2),
-      m_length2(ffd.scale * length / 2),
-      m_hitRadius((std::min)(m_width2, m_length2) / 2) {
-  if (auto xData = model.GetXData()) {
-    m_x = xData->GetValue();
-  }
-  if (auto yData = model.GetYData()) {
-    m_y = yData->GetValue();
-  }
-  if (auto rotationData = model.GetRotationData()) {
-    m_rot = rotationData->GetValue();
-  }
+      m_displayOptions{displayOptions},
+      m_width2(ffd.scale * displayOptions.width / 2),
+      m_length2(ffd.scale * displayOptions.length / 2),
+      m_hitRadius((std::min)(m_width2, m_length2) / 2),
+      m_pose{pose} {
   UpdateFrameData();
 }
 
-void ObjectFrameData::SetPosition(double x, double y) {
-  m_x = x;
-  m_y = y;
-  m_model.SetPosition(x, y);
+void PoseFrameData::SetPosition(const frc::Translation2d& pos) {
+  m_pose = frc::Pose2d{pos, m_pose.Rotation()};
+  m_model.SetPose(m_index, m_pose);
 }
 
-void ObjectFrameData::SetRotation(double rot) {
-  double rotDegrees = units::convert<units::radians, units::degrees>(rot);
-  // force to -180 to +180 range
-  rotDegrees = rotDegrees + std::ceil((-rotDegrees - 180) / 360) * 360;
-  m_rot = rotDegrees;
-  m_model.SetRotation(rotDegrees);
+void PoseFrameData::SetRotation(units::radian_t rot) {
+  m_pose = frc::Pose2d{m_pose.Translation(), rot};
+  m_model.SetPose(m_index, m_pose);
 }
 
-void ObjectFrameData::UpdateFrameData() {
+void PoseFrameData::UpdateFrameData() {
   // (0,0) origin is bottom left
-  ImVec2 center(m_ffd.min.x + m_ffd.scale * m_x,
-                m_ffd.max.y - m_ffd.scale * m_y);
+  ImVec2 center = m_ffd.GetScreenFromPos(m_pose.Translation());
 
   // build rotated points around center
   float length2 = m_length2;
   float width2 = m_width2;
-  double rot = GetRotation();
-  float cos_a = std::cos(-rot);
-  float sin_a = std::sin(-rot);
+  auto& rot = GetRotation();
+  float cos_a = rot.Cos();
+  float sin_a = -rot.Sin();
 
   m_corners[0] = center + ImRotate(ImVec2(-length2, -width2), cos_a, sin_a);
   m_corners[1] = center + ImRotate(ImVec2(length2, -width2), cos_a, sin_a);
   m_corners[2] = center + ImRotate(ImVec2(length2, width2), cos_a, sin_a);
   m_corners[3] = center + ImRotate(ImVec2(-length2, width2), cos_a, sin_a);
+  m_corners[4] = center + ImRotate(ImVec2(0, -width2), cos_a, sin_a);
+  m_corners[5] = center + ImRotate(ImVec2(0, width2), cos_a, sin_a);
+
+  float arrowScale = m_displayOptions.arrowSize / 100.0f;
   m_arrow[0] =
-      center + ImRotate(ImVec2(-length2 / 2, -width2 / 2), cos_a, sin_a);
-  m_arrow[1] = center + ImRotate(ImVec2(length2 / 2, 0), cos_a, sin_a);
+      center + ImRotate(ImVec2(-length2 * arrowScale, -width2 * arrowScale),
+                        cos_a, sin_a);
+  m_arrow[1] = center + ImRotate(ImVec2(length2 * arrowScale, 0), cos_a, sin_a);
   m_arrow[2] =
-      center + ImRotate(ImVec2(-length2 / 2, width2 / 2), cos_a, sin_a);
+      center + ImRotate(ImVec2(-length2 * arrowScale, width2 * arrowScale),
+                        cos_a, sin_a);
 
   m_center = center;
 }
 
-int ObjectFrameData::IsHovered(const ImVec2& cursor) const {
-  // only allow initiation of dragging when invisible button is hovered;
-  // this prevents the window resize handles from simultaneously activating
-  // the drag functionality
-  if (!ImGui::IsItemHovered()) {
-    return 0;
-  }
-
+std::pair<int, float> PoseFrameData::IsHovered(const ImVec2& cursor) const {
   float hitRadiusSquared = m_hitRadius * m_hitRadius;
+  float dist;
+
   // it's within the hit radius of the center?
-  if (gui::GetDistSquared(cursor, m_center) < hitRadiusSquared) {
-    return 1;
-  } else if (gui::GetDistSquared(cursor, m_corners[0]) < hitRadiusSquared) {
-    return 2;
-  } else if (gui::GetDistSquared(cursor, m_corners[1]) < hitRadiusSquared) {
-    return 3;
-  } else if (gui::GetDistSquared(cursor, m_corners[2]) < hitRadiusSquared) {
-    return 4;
-  } else if (gui::GetDistSquared(cursor, m_corners[3]) < hitRadiusSquared) {
-    return 5;
+  dist = gui::GetDistSquared(cursor, m_center);
+  if (dist < hitRadiusSquared) {
+    return {1, dist};
+  }
+
+  if (m_displayOptions.style == DisplayOptions::kBoxImage) {
+    dist = gui::GetDistSquared(cursor, m_corners[0]);
+    if (dist < hitRadiusSquared) {
+      return {2, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[1]);
+    if (dist < hitRadiusSquared) {
+      return {3, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[2]);
+    if (dist < hitRadiusSquared) {
+      return {4, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[3]);
+    if (dist < hitRadiusSquared) {
+      return {5, dist};
+    }
+  } else if (m_displayOptions.style == DisplayOptions::kTrack) {
+    dist = gui::GetDistSquared(cursor, m_corners[4]);
+    if (dist < hitRadiusSquared) {
+      return {6, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[5]);
+    if (dist < hitRadiusSquared) {
+      return {7, dist};
+    }
+  }
+
+  return {0, 0.0};
+}
+
+SelectedTargetInfo PoseFrameData::GetDragTarget(int corner, float dist) const {
+  SelectedTargetInfo info;
+  info.objModel = &m_model;
+  info.rot = GetRotation().Radians();
+  info.poseCenter = m_center;
+  if (corner == 1) {
+    info.center = m_center;
   } else {
-    return 0;
+    info.center = m_corners[corner - 2];
+  }
+  info.radius = m_hitRadius;
+  info.dist = dist;
+  info.corner = corner;
+  return info;
+}
+
+void PoseFrameData::HandleDrag(const ImVec2& cursor) {
+  if (gDragState.target.corner == 1) {
+    SetPosition(m_ffd.GetPosFromScreen(cursor - gDragState.initialOffset));
+    UpdateFrameData();
+    gDragState.target.center = m_center;
+    gDragState.target.poseCenter = m_center;
+  } else {
+    ImVec2 off = cursor - m_center;
+    SetRotation(gDragState.initialAngle -
+                units::radian_t{std::atan2(off.y, off.x)});
+    gDragState.target.center = m_corners[gDragState.target.corner - 2];
+    gDragState.target.rot = GetRotation().Radians();
   }
 }
 
-bool ObjectFrameData::HandleDrag(const ImVec2& cursor, int hitCorner,
-                                 ObjectDragState* drag) {
-  bool rv = false;
-  if (hitCorner > 0 && ImGui::IsMouseClicked(0)) {
-    if (hitCorner == 1) {
-      drag->corner = hitCorner;
-      drag->initialOffset = cursor - m_center;
-    } else {
-      drag->corner = hitCorner;
-      ImVec2 off = cursor - m_center;
-      drag->initialAngle = std::atan2(off.y, off.x) + GetRotation();
-    }
-    rv = true;
+void PoseFrameData::Draw(ImDrawList* drawList, std::vector<ImVec2>* center,
+                         std::vector<ImVec2>* left,
+                         std::vector<ImVec2>* right) const {
+  switch (m_displayOptions.style) {
+    case DisplayOptions::kBoxImage:
+      if (m_displayOptions.texture) {
+        drawList->AddImageQuad(m_displayOptions.texture, m_corners[0],
+                               m_corners[1], m_corners[2], m_corners[3]);
+        return;
+      }
+      drawList->AddQuad(m_corners[0], m_corners[1], m_corners[2], m_corners[3],
+                        m_displayOptions.color, m_displayOptions.weight);
+      break;
+    case DisplayOptions::kLine:
+    case DisplayOptions::kLineClosed:
+      center->emplace_back(m_center);
+      break;
+    case DisplayOptions::kTrack:
+      center->emplace_back(m_center);
+      left->emplace_back(m_corners[4]);
+      right->emplace_back(m_corners[5]);
+      break;
   }
 
-  if (drag->corner > 0 && ImGui::IsMouseDown(0)) {
-    if (drag->corner == 1) {
-      ImVec2 newPos = cursor - drag->initialOffset;
-      SetPosition(
-          (std::clamp(newPos.x, m_ffd.min.x, m_ffd.max.x) - m_ffd.min.x) /
-              m_ffd.scale,
-          (m_ffd.max.y - std::clamp(newPos.y, m_ffd.min.y, m_ffd.max.y)) /
-              m_ffd.scale);
-      UpdateFrameData();
-    } else {
-      ImVec2 off = cursor - m_center;
-      SetRotation(drag->initialAngle - std::atan2(off.y, off.x));
-    }
-  } else {
-    drag->corner = 0;
-  }
-
-  return rv;
-}
-
-void ObjectFrameData::Draw(ImDrawList* drawList, const gui::Texture& texture,
-                           int hitCorner) const {
-  if (texture) {
-    drawList->AddImageQuad(texture, m_corners[0], m_corners[1], m_corners[2],
-                           m_corners[3]);
-  } else {
-    drawList->AddQuad(m_corners[0], m_corners[1], m_corners[2], m_corners[3],
-                      IM_COL32(255, 0, 0, 255), 4.0);
+  if (m_displayOptions.arrows) {
     drawList->AddTriangle(m_arrow[0], m_arrow[1], m_arrow[2],
-                          IM_COL32(0, 255, 0, 255), 4.0);
-  }
-
-  if (hitCorner > 0) {
-    if (hitCorner == 1) {
-      drawList->AddCircle(m_center, m_hitRadius, IM_COL32(0, 255, 0, 255));
-    } else {
-      drawList->AddCircle(m_corners[hitCorner - 2], m_hitRadius,
-                          IM_COL32(0, 255, 0, 255));
-    }
+                          m_displayOptions.arrowColor,
+                          m_displayOptions.arrowWeight);
   }
 }
 
@@ -518,61 +862,343 @@ void glass::DisplayField2DSettings(Field2DModel* model) {
     field = storage.GetData<FieldInfo>();
   }
 
+  static const char* unitNames[] = {"meters", "feet", "inches"};
+  int* pDisplayUnits = GetStorage().GetIntRef("units", kDisplayMeters);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+  ImGui::Combo("Units", pDisplayUnits, unitNames, IM_ARRAYSIZE(unitNames));
+  gDisplayUnits = static_cast<DisplayUnits>(*pDisplayUnits);
+
   ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
   if (ImGui::CollapsingHeader("Field")) {
     ImGui::PushID("Field");
-    if (ImGui::Button("Choose image...")) {
-      field->m_fileOpener = std::make_unique<pfd::open_file>(
-          "Choose field image", "",
-          std::vector<std::string>{"Image File",
-                                   "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
-                                   "*.hdr *.pic *.ppm *.pgm",
-                                   "PathWeaver JSON File", "*.json"});
-    }
-    if (ImGui::Button("Reset image")) {
-      field->Reset();
-    }
-    ImGui::InputFloat("Field Width", field->m_pWidth);
-    ImGui::InputFloat("Field Height", field->m_pHeight);
-    // ImGui::InputInt("Field Top", field->m_pTop);
-    // ImGui::InputInt("Field Left", field->m_pLeft);
-    // ImGui::InputInt("Field Right", field->m_pRight);
-    // ImGui::InputInt("Field Bottom", field->m_pBottom);
+    field->DisplaySettings();
     ImGui::PopID();
   }
 
-  model->ForEachFieldObjectGroup([&](auto& groupModel, auto name) {
-    if (!groupModel.Exists()) {
+  model->ForEachFieldObject([&](auto& objModel, auto name) {
+    if (!objModel.Exists()) {
       return;
     }
     PushID(name);
-    auto& objGroupRef = field->m_objectGroups[name];
-    if (!objGroupRef) {
-      objGroupRef = std::make_unique<ObjectGroupInfo>();
+    auto& objRef = field->m_objects[name];
+    if (!objRef) {
+      objRef = std::make_unique<ObjectInfo>();
     }
-    auto objGroup = objGroupRef.get();
+    auto obj = objRef.get();
 
     wpi::SmallString<64> nameBuf = name;
     if (ImGui::CollapsingHeader(nameBuf.c_str())) {
-      if (ImGui::Button("Choose image...")) {
-        objGroup->m_fileOpener = std::make_unique<pfd::open_file>(
-            "Choose object image", "",
-            std::vector<std::string>{
-                "Image File",
-                "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
-                "*.hdr *.pic *.ppm *.pgm"});
-      }
-      if (ImGui::Button("Reset image")) {
-        objGroup->Reset();
-      }
-      ImGui::InputFloat("Width", objGroup->m_pWidth);
-      ImGui::InputFloat("Length", objGroup->m_pLength);
+      obj->DisplaySettings();
     }
     PopID();
   });
   ImGui::PopItemWidth();
 }
 
+namespace {
+class FieldDisplay {
+ public:
+  void Display(FieldInfo* field, Field2DModel* model,
+               const ImVec2& contentSize);
+
+ private:
+  void DisplayObject(FieldObjectModel& model, wpi::StringRef name);
+
+  FieldInfo* m_field;
+  ImVec2 m_mousePos;
+  ImDrawList* m_drawList;
+
+  // only allow initiation of dragging when invisible button is hovered;
+  // this prevents the window resize handles from simultaneously activating
+  // the drag functionality
+  bool m_isHovered;
+
+  FieldFrameData m_ffd;
+
+  // drag targets
+  std::vector<SelectedTargetInfo> m_targets;
+
+  // splitter so lines are put behind arrows
+  ImDrawListSplitter m_drawSplit;
+
+  // lines; static so buffer gets reused
+  std::vector<ImVec2> m_centerLine, m_leftLine, m_rightLine;
+};
+}  // namespace
+
+void FieldDisplay::Display(FieldInfo* field, Field2DModel* model,
+                           const ImVec2& contentSize) {
+  // screen coords
+  ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos();
+
+  // for dragging to work, there needs to be a button (otherwise the window is
+  // dragged)
+  ImGui::InvisibleButton("field", contentSize);
+
+  m_field = field;
+  m_mousePos = ImGui::GetIO().MousePos;
+  m_drawList = ImGui::GetWindowDrawList();
+  m_isHovered = ImGui::IsItemHovered();
+
+  // field
+  field->LoadImage();
+  m_ffd = field->GetFrameData(cursorPos, cursorPos + contentSize);
+  field->Draw(m_drawList, m_ffd);
+
+  // stop dragging if mouse button not down
+  bool isDown = ImGui::IsMouseDown(0);
+  if (!isDown) {
+    gDragState.target.objModel = nullptr;
+  }
+
+  // clear popup target if popup closed
+  bool isPopupOpen = ImGui::IsPopupOpen("edit");
+  if (!isPopupOpen) {
+    gPopupState.Close();
+  }
+
+  // field objects
+  m_targets.resize(0);
+  model->ForEachFieldObject([this](auto& objModel, auto name) {
+    if (objModel.Exists()) {
+      DisplayObject(objModel, name);
+    }
+  });
+
+  SelectedTargetInfo* target = nullptr;
+
+  if (gDragState.target.objModel) {
+    target = &gDragState.target;
+  } else if (gPopupState.GetTarget()->objModel) {
+    target = gPopupState.GetTarget();
+  } else if (!m_targets.empty()) {
+    // Find the "best" drag target of the available options.  Prefer
+    // center to non-center, and then pick the closest hit.
+    std::sort(m_targets.begin(), m_targets.end(),
+              [](const auto& a, const auto& b) {
+                return a.corner == 0 || a.dist < b.dist;
+              });
+    target = &m_targets.front();
+  }
+
+  if (target) {
+    // draw the target circle; also draw a smaller circle on the pose center
+    m_drawList->AddCircle(target->center, target->radius,
+                          IM_COL32(0, 255, 0, 255));
+    if (target->corner != 1) {
+      m_drawList->AddCircle(target->poseCenter, target->radius / 2.0,
+                            IM_COL32(0, 255, 0, 255));
+    }
+  }
+
+  // right-click popup for editing
+  if (m_isHovered && ImGui::IsMouseClicked(ImGuiMouseButton_Right)) {
+    gPopupState.Open(target, m_ffd.GetPosFromScreen(m_mousePos));
+    ImGui::OpenPopup("edit");
+  }
+  if (ImGui::BeginPopup("edit")) {
+    gPopupState.Display(model, m_ffd);
+    ImGui::EndPopup();
+  } else if (target) {
+    if (m_isHovered && ImGui::IsMouseClicked(0)) {
+      // initialize drag state
+      gDragState.target = *target;
+      gDragState.initialOffset = m_mousePos - target->poseCenter;
+      if (target->corner != 1) {
+        gDragState.initialAngle =
+            units::radian_t{std::atan2(gDragState.initialOffset.y,
+                                       gDragState.initialOffset.x)} +
+            target->rot;
+      }
+    }
+
+    // show tooltip and highlight
+    auto pos = m_ffd.GetPosFromScreen(target->poseCenter);
+    ImGui::SetTooltip(
+        "%s[%d]\nx: %0.3f y: %0.3f rot: %0.3f", target->name.c_str(),
+        static_cast<int>(target->index), ConvertDisplayLength(pos.X()),
+        ConvertDisplayLength(pos.Y()), ConvertDisplayAngle(target->rot));
+  }
+}
+
+void FieldDisplay::DisplayObject(FieldObjectModel& model, wpi::StringRef name) {
+  PushID(name);
+  auto& objRef = m_field->m_objects[name];
+  if (!objRef) {
+    objRef = std::make_unique<ObjectInfo>();
+  }
+  auto obj = objRef.get();
+  obj->LoadImage();
+
+  auto displayOptions = obj->GetDisplayOptions();
+
+  m_centerLine.resize(0);
+  m_leftLine.resize(0);
+  m_rightLine.resize(0);
+
+  m_drawSplit.Split(m_drawList, 2);
+  m_drawSplit.SetCurrentChannel(m_drawList, 1);
+  wpi::ArrayRef<frc::Pose2d> poses = gPopupState.GetInsertModel() == &model
+                                         ? gPopupState.GetInsertPoses()
+                                         : model.GetPoses();
+  size_t i = 0;
+  for (auto&& pose : poses) {
+    PoseFrameData pfd{pose, model, i, m_ffd, displayOptions};
+
+    // check for potential drag targets
+    if (displayOptions.selectable && m_isHovered &&
+        !gDragState.target.objModel) {
+      auto [corner, dist] = pfd.IsHovered(m_mousePos);
+      if (corner > 0) {
+        m_targets.emplace_back(pfd.GetDragTarget(corner, dist));
+        m_targets.back().name = name;
+        m_targets.back().index = i;
+      }
+    }
+
+    // handle active dragging of this object
+    if (gDragState.target.objModel == &model && gDragState.target.index == i) {
+      pfd.HandleDrag(m_mousePos);
+    }
+
+    // draw
+    pfd.Draw(m_drawList, &m_centerLine, &m_leftLine, &m_rightLine);
+    ++i;
+  }
+
+  m_drawSplit.SetCurrentChannel(m_drawList, 0);
+  obj->DrawLine(m_drawList, m_centerLine);
+  obj->DrawLine(m_drawList, m_leftLine);
+  obj->DrawLine(m_drawList, m_rightLine);
+  m_drawSplit.Merge(m_drawList);
+
+  PopID();
+}
+
+void PopupState::Open(SelectedTargetInfo* target,
+                      const frc::Translation2d& pos) {
+  if (target) {
+    m_target = *target;
+  } else {
+    m_target.objModel = nullptr;
+    m_insertModel = nullptr;
+    m_insertPoses.resize(0);
+    m_insertPoses.emplace_back(pos, 0_deg);
+    m_insertName.clear();
+    m_insertIndex = 0;
+  }
+}
+
+void PopupState::Close() {
+  m_target.objModel = nullptr;
+  m_insertModel = nullptr;
+  m_insertPoses.resize(0);
+}
+
+void PopupState::Display(Field2DModel* model, const FieldFrameData& ffd) {
+  if (m_target.objModel) {
+    DisplayTarget(model, ffd);
+  } else {
+    DisplayInsert(model);
+  }
+}
+
+void PopupState::DisplayTarget(Field2DModel* model, const FieldFrameData& ffd) {
+  ImGui::Text("%s[%d]", m_target.name.c_str(),
+              static_cast<int>(m_target.index));
+  frc::Pose2d pose{ffd.GetPosFromScreen(m_target.poseCenter), m_target.rot};
+  if (InputPose(&pose)) {
+    m_target.poseCenter = ffd.GetScreenFromPos(pose.Translation());
+    m_target.rot = pose.Rotation().Radians();
+    m_target.objModel->SetPose(m_target.index, pose);
+  }
+  if (ImGui::Button("Delete Pose")) {
+    std::vector<frc::Pose2d> poses = m_target.objModel->GetPoses();
+    if (m_target.index < poses.size()) {
+      poses.erase(poses.begin() + m_target.index);
+      m_target.objModel->SetPoses(poses);
+    }
+    ImGui::CloseCurrentPopup();
+  }
+  if (ImGui::Button("Delete Object (ALL Poses)")) {
+    model->RemoveFieldObject(m_target.name);
+    ImGui::CloseCurrentPopup();
+  }
+}
+
+void PopupState::DisplayInsert(Field2DModel* model) {
+  ImGui::TextUnformatted("Insert New Pose");
+
+  InputPose(&m_insertPoses[m_insertIndex]);
+
+  const char* insertName = m_insertModel ? m_insertName.c_str() : "<new>";
+  if (ImGui::BeginCombo("Object", insertName)) {
+    bool selected = !m_insertModel;
+    if (ImGui::Selectable("<new>", selected)) {
+      m_insertModel = nullptr;
+      auto pose = m_insertPoses[m_insertIndex];
+      m_insertPoses.resize(0);
+      m_insertPoses.emplace_back(std::move(pose));
+      m_insertName.clear();
+      m_insertIndex = 0;
+    }
+    if (selected) {
+      ImGui::SetItemDefaultFocus();
+    }
+    model->ForEachFieldObject([&](auto& objModel, auto name) {
+      bool selected = m_insertModel == &objModel;
+      if (ImGui::Selectable(name.data(), selected)) {
+        m_insertModel = &objModel;
+        auto pose = m_insertPoses[m_insertIndex];
+        m_insertPoses = objModel.GetPoses();
+        m_insertPoses.emplace_back(std::move(pose));
+        m_insertName = name;
+        m_insertIndex = m_insertPoses.size() - 1;
+      }
+      if (selected) {
+        ImGui::SetItemDefaultFocus();
+      }
+    });
+    ImGui::EndCombo();
+  }
+  if (m_insertModel) {
+    int oldIndex = m_insertIndex;
+    if (ImGui::InputInt("Pos", &m_insertIndex, 1, 5)) {
+      if (m_insertIndex < 0) {
+        m_insertIndex = 0;
+      }
+      size_t size = m_insertPoses.size();
+      if (static_cast<size_t>(m_insertIndex) >= size) {
+        m_insertIndex = size - 1;
+      }
+      if (m_insertIndex < oldIndex) {
+        auto begin = m_insertPoses.begin();
+        std::rotate(begin + m_insertIndex, begin + oldIndex,
+                    begin + oldIndex + 1);
+      } else if (m_insertIndex > oldIndex) {
+        auto rbegin = m_insertPoses.rbegin();
+        std::rotate(rbegin + (size - m_insertIndex), rbegin + (size - oldIndex),
+                    rbegin + (size - oldIndex - 1));
+      }
+    }
+  } else {
+    ImGui::InputText("Name", &m_insertName);
+  }
+
+  if (ImGui::Button("Apply")) {
+    if (m_insertModel) {
+      m_insertModel->SetPoses(m_insertPoses);
+    } else if (!m_insertName.empty()) {
+      model->AddFieldObject(m_insertName)->SetPoses(m_insertPoses);
+    }
+    ImGui::CloseCurrentPopup();
+  }
+  ImGui::SameLine();
+  if (ImGui::Button("Cancel")) {
+    ImGui::CloseCurrentPopup();
+  }
+}
+
 void glass::DisplayField2D(Field2DModel* model, const ImVec2& contentSize) {
   auto& storage = GetStorage();
   auto field = storage.GetData<FieldInfo>();
@@ -581,55 +1207,12 @@ void glass::DisplayField2D(Field2DModel* model, const ImVec2& contentSize) {
     field = storage.GetData<FieldInfo>();
   }
 
-  ImVec2 windowPos = ImGui::GetWindowPos();
-  ImVec2 mousePos = ImGui::GetIO().MousePos;
-
-  // for dragging to work, there needs to be a button (otherwise the window is
-  // dragged)
   if (contentSize.x <= 0 || contentSize.y <= 0) {
     return;
   }
-  ImVec2 cursorPos = windowPos + ImGui::GetCursorPos();  // screen coords
-  ImGui::InvisibleButton("field", contentSize);
 
-  // field
-  field->LoadImage();
-  FieldFrameData ffd = field->GetFrameData(cursorPos, cursorPos + contentSize);
-  auto drawList = ImGui::GetWindowDrawList();
-  field->Draw(drawList, ffd);
-
-  model->ForEachFieldObjectGroup([&](auto& groupModel, auto name) {
-    if (!groupModel.Exists()) {
-      return;
-    }
-    PushID(name);
-    auto& objGroupRef = field->m_objectGroups[name];
-    if (!objGroupRef) {
-      objGroupRef = std::make_unique<ObjectGroupInfo>();
-    }
-    auto objGroup = objGroupRef.get();
-    objGroup->LoadImage();
-
-    int i = 0;
-    groupModel.ForEachFieldObject([&](auto& objModel) {
-      ++i;
-      ObjectFrameData ofd{objModel, ffd, *objGroup->m_pWidth,
-                          *objGroup->m_pLength};
-
-      int hitCorner = 0;
-      if (objGroup->m_dragState.object == 0 ||
-          objGroup->m_dragState.object == i) {
-        hitCorner = ofd.IsHovered(mousePos);
-        if (ofd.HandleDrag(mousePos, hitCorner, &objGroup->m_dragState)) {
-          objGroup->m_dragState.object = i;
-        }
-      }
-
-      // draw
-      ofd.Draw(drawList, objGroup->GetTexture(), hitCorner);
-    });
-    PopID();
-  });
+  static FieldDisplay display;
+  display.Display(field, model, contentSize);
 }
 
 void Field2DView::Display() {
diff --git a/glass/src/lib/native/cpp/other/Log.cpp b/glass/src/lib/native/cpp/other/Log.cpp
index 83bbe3778f..8e6db5d4aa 100644
--- a/glass/src/lib/native/cpp/other/Log.cpp
+++ b/glass/src/lib/native/cpp/other/Log.cpp
@@ -23,7 +23,7 @@ void LogData::Append(const wpi::Twine& msg) {
   }
 
   size_t oldSize = m_buf.size();
-  wpi::raw_vector_ostream os{m_buf};
+  wpi::raw_string_ostream os{m_buf};
   msg.print(os);
   for (size_t newSize = m_buf.size(); oldSize < newSize; ++oldSize) {
     if (m_buf[oldSize] == '\n') {
@@ -32,6 +32,10 @@ void LogData::Append(const wpi::Twine& msg) {
   }
 }
 
+const std::string& LogData::GetBuffer() {
+  return m_buf;
+}
+
 void glass::DisplayLog(LogData* data, bool autoScroll) {
   if (data->m_buf.empty()) {
     return;
@@ -65,6 +69,11 @@ void LogView::Display() {
     if (ImGui::Selectable("Clear")) {
       m_data->Clear();
     }
+    const auto& buf = m_data->GetBuffer();
+    if (ImGui::Selectable("Copy to Clipboard", false,
+                          buf.empty() ? ImGuiSelectableFlags_Disabled : 0)) {
+      ImGui::SetClipboardText(buf.c_str());
+    }
     ImGui::EndPopup();
   }
 
diff --git a/glass/src/lib/native/cpp/other/Plot.cpp b/glass/src/lib/native/cpp/other/Plot.cpp
index d850b626dc..ebb132aef2 100644
--- a/glass/src/lib/native/cpp/other/Plot.cpp
+++ b/glass/src/lib/native/cpp/other/Plot.cpp
@@ -174,6 +174,7 @@ PlotSeries::PlotSeries(wpi::StringRef id) : m_id(id) {
 
 PlotSeries::PlotSeries(DataSource* source, int yAxis) : m_yAxis(yAxis) {
   SetSource(source);
+  m_id = source->GetId();
 }
 
 void PlotSeries::CheckSource() {
@@ -191,7 +192,6 @@ void PlotSeries::CheckSource() {
 
 void PlotSeries::SetSource(DataSource* source) {
   m_source = source;
-  m_id = source->GetId();
 
   // add initial value
   m_data[m_size++] = ImPlotPoint{wpi::Now() * 1.0e-6, source->GetValue()};
@@ -374,9 +374,15 @@ PlotSeries::Action PlotSeries::EmitPlot(PlotView& view, double now, size_t i,
     ImPlot::EndLegendDragDropSource();
   }
 
+  // Show full source name tooltip
+  if (!m_name.empty() && ImPlot::IsLegendEntryHovered(label)) {
+    ImGui::SetTooltip("%s", m_id.c_str());
+  }
+
   // Edit settings via popup
   Action rv = kNone;
   if (ImPlot::BeginLegendPopup(label)) {
+    ImGui::TextUnformatted(m_id.c_str());
     if (ImGui::Button("Close")) {
       ImGui::CloseCurrentPopup();
     }
@@ -983,10 +989,25 @@ void PlotProvider::DisplayMenu() {
   }
 
   if (ImGui::MenuItem("New Plot Window")) {
+    // this is an inefficient algorithm, but the number of windows is small
     char id[32];
-    std::snprintf(id, sizeof(id), "Plot <%d>",
-                  static_cast<int>(m_windows.size()));
-    AddWindow(id, std::make_unique<PlotView>(this));
+    size_t numWindows = m_windows.size();
+    for (size_t i = 0; i <= numWindows; ++i) {
+      std::snprintf(id, sizeof(id), "Plot <%d>", static_cast<int>(i));
+      bool match = false;
+      for (size_t j = i; j < numWindows; ++j) {
+        if (m_windows[j]->GetId() == id) {
+          match = true;
+          break;
+        }
+      }
+      if (!match) {
+        break;
+      }
+    }
+    if (auto win = AddWindow(id, std::make_unique<PlotView>(this))) {
+      win->SetDefaultSize(700, 400);
+    }
   }
 }
 
diff --git a/glass/src/lib/native/cpp/other/StringChooser.cpp b/glass/src/lib/native/cpp/other/StringChooser.cpp
index 98bd3417d9..46fa674356 100644
--- a/glass/src/lib/native/cpp/other/StringChooser.cpp
+++ b/glass/src/lib/native/cpp/other/StringChooser.cpp
@@ -38,6 +38,4 @@ void glass::DisplayStringChooser(StringChooserModel* model) {
     }
     ImGui::EndCombo();
   }
-
-  ImGui::SameLine();
 }
diff --git a/glass/src/lib/native/include/glass/other/Field2D.h b/glass/src/lib/native/include/glass/other/Field2D.h
index 37a52bb837..35d188a97d 100644
--- a/glass/src/lib/native/include/glass/other/Field2D.h
+++ b/glass/src/lib/native/include/glass/other/Field2D.h
@@ -4,38 +4,37 @@
 
 #pragma once
 
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
 #include <imgui.h>
+#include <wpi/ArrayRef.h>
 #include <wpi/STLExtras.h>
 #include <wpi/StringRef.h>
+#include <wpi/Twine.h>
 
 #include "glass/Model.h"
 #include "glass/View.h"
 
 namespace glass {
 
-class DataSource;
-
 class FieldObjectModel : public Model {
  public:
-  virtual DataSource* GetXData() = 0;
-  virtual DataSource* GetYData() = 0;
-  virtual DataSource* GetRotationData() = 0;
+  virtual const char* GetName() const = 0;
 
-  virtual void SetPose(double x, double y, double rot) = 0;
-  virtual void SetPosition(double x, double y) = 0;
-  virtual void SetRotation(double rot) = 0;
-};
-
-class FieldObjectGroupModel : public Model {
- public:
-  virtual void ForEachFieldObject(
-      wpi::function_ref<void(FieldObjectModel& model)> func) = 0;
+  virtual wpi::ArrayRef<frc::Pose2d> GetPoses() = 0;
+  virtual void SetPoses(wpi::ArrayRef<frc::Pose2d> poses) = 0;
+  virtual void SetPose(size_t i, frc::Pose2d pose) = 0;
+  virtual void SetPosition(size_t i, frc::Translation2d pos) = 0;
+  virtual void SetRotation(size_t i, frc::Rotation2d rot) = 0;
 };
 
 class Field2DModel : public Model {
  public:
-  virtual void ForEachFieldObjectGroup(
-      wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
+  virtual FieldObjectModel* AddFieldObject(const wpi::Twine& name) = 0;
+  virtual void RemoveFieldObject(const wpi::Twine& name) = 0;
+  virtual void ForEachFieldObject(
+      wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
           func) = 0;
 };
 
diff --git a/glass/src/lib/native/include/glass/other/Log.h b/glass/src/lib/native/include/glass/other/Log.h
index 3d1ab146a1..ddaadaf918 100644
--- a/glass/src/lib/native/include/glass/other/Log.h
+++ b/glass/src/lib/native/include/glass/other/Log.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <string>
 #include <vector>
 
 #include <wpi/Twine.h>
@@ -20,10 +21,11 @@ class LogData {
 
   void Clear();
   void Append(const wpi::Twine& msg);
+  const std::string& GetBuffer();
 
  private:
   size_t m_maxLines;
-  std::vector<char> m_buf;
+  std::string m_buf;
   std::vector<size_t> m_lineOffsets{0};
 };
 
diff --git a/glass/src/libnt/native/cpp/NTField2D.cpp b/glass/src/libnt/native/cpp/NTField2D.cpp
index 073aff9b8f..89f6a639e5 100644
--- a/glass/src/libnt/native/cpp/NTField2D.cpp
+++ b/glass/src/libnt/native/cpp/NTField2D.cpp
@@ -5,20 +5,22 @@
 #include "glass/networktables/NTField2D.h"
 
 #include <algorithm>
+#include <vector>
 
 #include <ntcore_cpp.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+#include <wpi/SmallString.h>
 #include <wpi/SmallVector.h>
 
-#include "glass/DataSource.h"
-
 using namespace glass;
 
-class NTField2DModel::GroupModel : public FieldObjectGroupModel {
+class NTField2DModel::ObjectModel : public FieldObjectModel {
  public:
-  GroupModel(wpi::StringRef name, NT_Entry entry)
+  ObjectModel(wpi::StringRef name, NT_Entry entry)
       : m_name{name}, m_entry{entry} {}
 
-  wpi::StringRef GetName() const { return m_name; }
+  const char* GetName() const override { return m_name.c_str(); }
   NT_Entry GetEntry() const { return m_entry; }
 
   void NTUpdate(const nt::Value& value);
@@ -31,146 +33,118 @@ class NTField2DModel::GroupModel : public FieldObjectGroupModel {
   bool Exists() override { return nt::GetEntryType(m_entry) != NT_UNASSIGNED; }
   bool IsReadOnly() override { return false; }
 
-  void ForEachFieldObject(
-      wpi::function_ref<void(FieldObjectModel& model)> func) override;
+  wpi::ArrayRef<frc::Pose2d> GetPoses() override { return m_poses; }
+  void SetPoses(wpi::ArrayRef<frc::Pose2d> poses) override;
+  void SetPose(size_t i, frc::Pose2d pose) override;
+  void SetPosition(size_t i, frc::Translation2d pos) override;
+  void SetRotation(size_t i, frc::Rotation2d rot) override;
 
  private:
+  void UpdateNT();
+
   std::string m_name;
   NT_Entry m_entry;
 
-  // keep count of objects rather than resizing vector, as there is a fair
-  // amount of overhead associated with the latter (DataSource record keeping)
-  size_t m_count = 0;
-  class ObjectModel;
-  std::vector<std::unique_ptr<ObjectModel>> m_objects;
+  std::vector<frc::Pose2d> m_poses;
 };
 
-class NTField2DModel::GroupModel::ObjectModel : public FieldObjectModel {
- public:
-  ObjectModel(wpi::StringRef name, NT_Entry entry, int index)
-      : m_entry{entry},
-        m_index{index},
-        m_x{name + "[" + wpi::Twine{index} + "]/x"},
-        m_y{name + "[" + wpi::Twine{index} + "]/y"},
-        m_rot{name + "[" + wpi::Twine{index} + "]/rot"} {}
+void NTField2DModel::ObjectModel::NTUpdate(const nt::Value& value) {
+  if (value.IsDoubleArray()) {
+    auto arr = value.GetDoubleArray();
+    auto size = arr.size();
+    if ((size % 3) != 0) {
+      return;
+    }
+    m_poses.resize(size / 3);
+    for (size_t i = 0; i < size / 3; ++i) {
+      m_poses[i] = frc::Pose2d{
+          units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+          frc::Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+    }
+  } else if (value.IsRaw()) {
+    // treat it simply as an array of doubles
+    wpi::StringRef data = value.GetRaw();
 
-  void SetExists(bool exists) { m_exists = exists; }
-
-  void Update() override {}
-  bool Exists() override { return m_exists; }
-  bool IsReadOnly() override { return false; }
-
-  DataSource* GetXData() override { return &m_x; }
-  DataSource* GetYData() override { return &m_y; }
-  DataSource* GetRotationData() override { return &m_rot; }
-
-  void SetPose(double x, double y, double rot) override;
-  void SetPosition(double x, double y) override;
-  void SetRotation(double rot) override;
-
- private:
-  void SetPoseImpl(double x, double y, double rot, bool setX, bool setY,
-                   bool setRot);
-
-  NT_Entry m_entry;
-  int m_index;
-  bool m_exists = true;
-
- public:
-  DataSource m_x;
-  DataSource m_y;
-  DataSource m_rot;
-};
-
-void NTField2DModel::GroupModel::NTUpdate(const nt::Value& value) {
-  if (!value.IsDoubleArray()) {
-    m_count = 0;
-    return;
-  }
-
-  auto arr = value.GetDoubleArray();
-  // must be triples
-  if ((arr.size() % 3) != 0) {
-    m_count = 0;
-    return;
-  }
-
-  m_count = arr.size() / 3;
-  if (m_count > m_objects.size()) {
-    m_objects.reserve(m_count);
-    for (size_t i = m_objects.size(); i < m_count; ++i) {
-      m_objects.emplace_back(std::make_unique<ObjectModel>(m_name, m_entry, i));
+    // must be triples of doubles
+    auto size = data.size();
+    if ((size % (3 * 8)) != 0) {
+      return;
+    }
+    m_poses.resize(size / (3 * 8));
+    const char* p = data.begin();
+    for (size_t i = 0; i < size / (3 * 8); ++i) {
+      double x = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double y = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double rot = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      m_poses[i] = frc::Pose2d{units::meter_t{x}, units::meter_t{y},
+                               frc::Rotation2d{units::degree_t{rot}}};
     }
   }
-  if (m_count < m_objects.size()) {
-    for (size_t i = m_count; i < m_objects.size(); ++i) {
-      m_objects[i]->SetExists(false);
+}
+
+void NTField2DModel::ObjectModel::UpdateNT() {
+  if (m_poses.size() < (255 / 3)) {
+    wpi::SmallVector<double, 9> arr;
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      arr.push_back(translation.X().to<double>());
+      arr.push_back(translation.Y().to<double>());
+      arr.push_back(pose.Rotation().Degrees().to<double>());
     }
-  }
-
-  for (size_t i = 0; i < m_count; ++i) {
-    auto& obj = m_objects[i];
-    obj->SetExists(true);
-    obj->m_x.SetValue(arr[i * 3], value.last_change());
-    obj->m_y.SetValue(arr[i * 3 + 1], value.last_change());
-    obj->m_rot.SetValue(arr[i * 3 + 2], value.last_change());
+    nt::SetEntryTypeValue(m_entry, nt::Value::MakeDoubleArray(arr));
+  } else {
+    // send as raw array of doubles if too big for NT array
+    std::vector<char> arr;
+    arr.resize(m_poses.size() * 3 * 8);
+    char* p = arr.data();
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.X().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.Y().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(pose.Rotation().Degrees().to<double>()));
+      p += 8;
+    }
+    nt::SetEntryTypeValue(
+        m_entry, nt::Value::MakeRaw(wpi::StringRef{arr.data(), arr.size()}));
   }
 }
 
-void NTField2DModel::GroupModel::ForEachFieldObject(
-    wpi::function_ref<void(FieldObjectModel& model)> func) {
-  for (size_t i = 0; i < m_count; ++i) {
-    func(*m_objects[i]);
+void NTField2DModel::ObjectModel::SetPoses(wpi::ArrayRef<frc::Pose2d> poses) {
+  m_poses = poses;
+  UpdateNT();
+}
+
+void NTField2DModel::ObjectModel::SetPose(size_t i, frc::Pose2d pose) {
+  if (i < m_poses.size()) {
+    m_poses[i] = pose;
+    UpdateNT();
   }
 }
 
-void NTField2DModel::GroupModel::ObjectModel::SetPose(double x, double y,
-                                                      double rot) {
-  SetPoseImpl(x, y, rot, true, true, true);
+void NTField2DModel::ObjectModel::SetPosition(size_t i,
+                                              frc::Translation2d pos) {
+  if (i < m_poses.size()) {
+    m_poses[i] = frc::Pose2d{pos, m_poses[i].Rotation()};
+    UpdateNT();
+  }
 }
 
-void NTField2DModel::GroupModel::ObjectModel::SetPosition(double x, double y) {
-  SetPoseImpl(x, y, 0, true, true, false);
-}
-
-void NTField2DModel::GroupModel::ObjectModel::SetRotation(double rot) {
-  SetPoseImpl(0, 0, rot, false, false, true);
-}
-
-void NTField2DModel::GroupModel::ObjectModel::SetPoseImpl(double x, double y,
-                                                          double rot, bool setX,
-                                                          bool setY,
-                                                          bool setRot) {
-  // get from NT, validate type and size
-  auto value = nt::GetEntryValue(m_entry);
-  if (!value || !value->IsDoubleArray()) {
-    return;
+void NTField2DModel::ObjectModel::SetRotation(size_t i, frc::Rotation2d rot) {
+  if (i < m_poses.size()) {
+    m_poses[i] = frc::Pose2d{m_poses[i].Translation(), rot};
+    UpdateNT();
   }
-  auto origArr = value->GetDoubleArray();
-  if (static_cast<int>(origArr.size()) < ((m_index + 1) * 3)) {
-    return;
-  }
-
-  // copy existing array
-  wpi::SmallVector<double, 8> arr;
-  arr.reserve(origArr.size());
-  for (auto&& elem : origArr) {
-    arr.emplace_back(elem);
-  }
-
-  // update value
-  if (setX) {
-    arr[m_index * 3 + 0] = x;
-  }
-  if (setY) {
-    arr[m_index * 3 + 1] = y;
-  }
-  if (setRot) {
-    arr[m_index * 3 + 2] = rot;
-  }
-
-  // set back to NT
-  nt::SetEntryValue(m_entry, nt::Value::MakeDoubleArray(arr));
 }
 
 NTField2DModel::NTField2DModel(wpi::StringRef path)
@@ -199,9 +173,9 @@ void NTField2DModel::Update() {
     // common case: update of existing entry; search by entry
     if (event.flags & NT_NOTIFY_UPDATE) {
       auto it = std::find_if(
-          m_groups.begin(), m_groups.end(),
+          m_objects.begin(), m_objects.end(),
           [&](const auto& e) { return e->GetEntry() == event.entry; });
-      if (it != m_groups.end()) {
+      if (it != m_objects.end()) {
         (*it)->NTUpdate(*event.value);
         continue;
       }
@@ -213,20 +187,16 @@ void NTField2DModel::Update() {
       if (name.empty() || name[0] == '.') {
         continue;
       }
-      auto it = std::lower_bound(m_groups.begin(), m_groups.end(), event.name,
-                                 [](const auto& e, wpi::StringRef name) {
-                                   return e->GetName() < name;
-                                 });
-      bool match = (it != m_groups.end() && (*it)->GetName() == event.name);
+      auto [it, match] = Find(event.name);
       if (event.flags & NT_NOTIFY_DELETE) {
         if (match) {
-          m_groups.erase(it);
+          m_objects.erase(it);
         }
         continue;
       } else if (event.flags & NT_NOTIFY_NEW) {
         if (!match) {
-          it = m_groups.emplace(
-              it, std::make_unique<GroupModel>(event.name, event.entry));
+          it = m_objects.emplace(
+              it, std::make_unique<ObjectModel>(event.name, event.entry));
         }
       } else if (!match) {
         continue;
@@ -246,12 +216,40 @@ bool NTField2DModel::IsReadOnly() {
   return false;
 }
 
-void NTField2DModel::ForEachFieldObjectGroup(
-    wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
+FieldObjectModel* NTField2DModel::AddFieldObject(const wpi::Twine& name) {
+  wpi::SmallString<128> fullNameBuf;
+  wpi::StringRef fullName = (m_path + name).toStringRef(fullNameBuf);
+  auto [it, match] = Find(fullName);
+  if (!match) {
+    it = m_objects.emplace(
+        it, std::make_unique<ObjectModel>(fullName, m_nt.GetEntry(fullName)));
+  }
+  return it->get();
+}
+
+void NTField2DModel::RemoveFieldObject(const wpi::Twine& name) {
+  wpi::SmallString<128> fullNameBuf;
+  auto [it, match] = Find((m_path + name).toStringRef(fullNameBuf));
+  if (match) {
+    nt::DeleteEntry((*it)->GetEntry());
+    m_objects.erase(it);
+  }
+}
+
+void NTField2DModel::ForEachFieldObject(
+    wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
         func) {
-  for (auto&& group : m_groups) {
-    if (group->Exists()) {
-      func(*group, wpi::StringRef{group->GetName()}.drop_front(m_path.size()));
+  for (auto&& obj : m_objects) {
+    if (obj->Exists()) {
+      func(*obj, wpi::StringRef{obj->GetName()}.drop_front(m_path.size()));
     }
   }
 }
+
+std::pair<NTField2DModel::Objects::iterator, bool> NTField2DModel::Find(
+    wpi::StringRef fullName) {
+  auto it = std::lower_bound(
+      m_objects.begin(), m_objects.end(), fullName,
+      [](const auto& e, wpi::StringRef name) { return e->GetName() < name; });
+  return {it, it != m_objects.end() && (*it)->GetName() == fullName};
+}
diff --git a/glass/src/libnt/native/cpp/NTStringChooser.cpp b/glass/src/libnt/native/cpp/NTStringChooser.cpp
index 33c8fa2d0b..e1c8d3bf18 100644
--- a/glass/src/libnt/native/cpp/NTStringChooser.cpp
+++ b/glass/src/libnt/native/cpp/NTStringChooser.cpp
@@ -39,17 +39,30 @@ void NTStringChooserModel::SetOptions(wpi::ArrayRef<std::string> val) {
 
 void NTStringChooserModel::Update() {
   for (auto&& event : m_nt.PollListener()) {
-    if (event.entry == m_default && event.value && event.value->IsString()) {
-      m_defaultValue = event.value->GetString();
-    } else if (event.entry == m_selected && event.value &&
-               event.value->IsString()) {
-      m_selectedValue = event.value->GetString();
-    } else if (event.entry == m_active && event.value &&
-               event.value->IsString()) {
-      m_activeValue = event.value->GetString();
-    } else if (event.entry == m_options && event.value &&
-               event.value->IsStringArray()) {
-      m_optionsValue = event.value->GetStringArray();
+    if (event.entry == m_default) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_defaultValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_defaultValue = event.value->GetString();
+      }
+    } else if (event.entry == m_selected) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_selectedValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_selectedValue = event.value->GetString();
+      }
+    } else if (event.entry == m_active) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_activeValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_activeValue = event.value->GetString();
+      }
+    } else if (event.entry == m_options) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_optionsValue.clear();
+      } else if (event.value && event.value->IsStringArray()) {
+        m_optionsValue = event.value->GetStringArray();
+      }
     }
   }
 }
diff --git a/glass/src/libnt/native/include/glass/networktables/NTField2D.h b/glass/src/libnt/native/include/glass/networktables/NTField2D.h
index 7414e37c74..c44b2a13a9 100644
--- a/glass/src/libnt/native/include/glass/networktables/NTField2D.h
+++ b/glass/src/libnt/native/include/glass/networktables/NTField2D.h
@@ -6,6 +6,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include <ntcore_cpp.h>
@@ -32,8 +33,10 @@ class NTField2DModel : public Field2DModel {
   bool Exists() override;
   bool IsReadOnly() override;
 
-  void ForEachFieldObjectGroup(
-      wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
+  FieldObjectModel* AddFieldObject(const wpi::Twine& name) override;
+  void RemoveFieldObject(const wpi::Twine& name) override;
+  void ForEachFieldObject(
+      wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
           func) override;
 
  private:
@@ -42,8 +45,11 @@ class NTField2DModel : public Field2DModel {
   NT_Entry m_name;
   std::string m_nameValue;
 
-  class GroupModel;
-  std::vector<std::unique_ptr<GroupModel>> m_groups;
+  class ObjectModel;
+  using Objects = std::vector<std::unique_ptr<ObjectModel>>;
+  Objects m_objects;
+
+  std::pair<Objects::iterator, bool> Find(wpi::StringRef fullName);
 };
 
 }  // namespace glass
diff --git a/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h b/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h
index 373b283233..b241c6efb8 100644
--- a/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h
+++ b/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h
@@ -28,7 +28,8 @@ class NetworkTablesHelper {
   }
 
   static constexpr int kDefaultListenerFlags =
-      NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE;
+      NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
+      NT_NOTIFY_IMMEDIATE;
 
   NT_EntryListener AddListener(NT_Entry entry,
                                unsigned int flags = kDefaultListenerFlags) {
diff --git a/wpigui/src/main/native/cpp/wpigui.cpp b/wpigui/src/main/native/cpp/wpigui.cpp
index 9deb9e7206..3843522e55 100644
--- a/wpigui/src/main/native/cpp/wpigui.cpp
+++ b/wpigui/src/main/native/cpp/wpigui.cpp
@@ -213,7 +213,23 @@ bool gui::Initialize(const char* title, int width, int height) {
 
   // Update window settings
   if (gContext->xPos != -1 && gContext->yPos != -1) {
-    glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    // check to make sure the position isn't off-screen
+    bool found = false;
+    int monCount;
+    GLFWmonitor** monitors = glfwGetMonitors(&monCount);
+    for (int i = 0; i < monCount; ++i) {
+      int monXPos, monYPos, monWidth, monHeight;
+      glfwGetMonitorWorkarea(monitors[i], &monXPos, &monYPos, &monWidth,
+                             &monHeight);
+      if (gContext->xPos >= monXPos && gContext->xPos < (monXPos + monWidth) &&
+          gContext->yPos >= monYPos && gContext->yPos < (monYPos + monHeight)) {
+        found = true;
+        break;
+      }
+    }
+    if (found) {
+      glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    }
     glfwShowWindow(gContext->window);
   }
 
diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
index 20fb1b57ba..f82c929b74 100644
--- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -12,6 +12,9 @@
 
 using namespace frc;
 
+AnalogEncoder::AnalogEncoder(int channel)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel)) {}
+
 AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
     : m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
       m_analogTrigger{m_analogInput.get()},
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index d0654866af..52968403ad 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -125,14 +125,10 @@ int Compressor::GetModule() const {
 void Compressor::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Compressor");
   builder.AddBooleanProperty(
-      "Enabled", [=]() { return Enabled(); },
-      [=](bool value) {
-        if (value) {
-          Start();
-        } else {
-          Stop();
-        }
-      });
+      "Closed Loop Control", [=]() { return GetClosedLoopControl(); },
+      [=](bool value) { SetClosedLoopControl(value); });
+  builder.AddBooleanProperty(
+      "Enabled", [=] { return Enabled(); }, nullptr);
   builder.AddBooleanProperty(
       "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
index d5b3833fe5..a4b11f247e 100644
--- a/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -34,6 +34,13 @@ void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
 ChassisSpeeds HolonomicDriveController::Calculate(
     const Pose2d& currentPose, const Pose2d& poseRef,
     units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+  // If this is the first run, then we need to reset the theta controller to the
+  // current pose's heading.
+  if (m_firstRun) {
+    m_thetaController.Reset(currentPose.Rotation().Radians());
+    m_firstRun = false;
+  }
+
   // Calculate feedforward velocities (field-relative)
   auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
   auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
index 9fa3ebebbb..2ea8237234 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
@@ -4,6 +4,13 @@
 
 #include "frc/smartdashboard/FieldObject2d.h"
 
+#include <vector>
+
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+
+#include "frc/trajectory/Trajectory.h"
+
 using namespace frc;
 
 FieldObject2d::FieldObject2d(FieldObject2d&& rhs) {
@@ -48,6 +55,16 @@ void FieldObject2d::SetPoses(std::initializer_list<Pose2d> poses) {
   SetPoses(wpi::makeArrayRef(poses.begin(), poses.end()));
 }
 
+void FieldObject2d::SetTrajectory(const Trajectory& trajectory) {
+  std::scoped_lock lock(m_mutex);
+  m_poses.clear();
+  m_poses.reserve(trajectory.States().size());
+  for (auto&& state : trajectory.States()) {
+    m_poses.push_back(state.pose);
+  }
+  UpdateEntry();
+}
+
 std::vector<Pose2d> FieldObject2d::GetPoses() const {
   std::scoped_lock lock(m_mutex);
   UpdateFromEntry();
@@ -66,17 +83,41 @@ void FieldObject2d::UpdateEntry(bool setDefault) {
   if (!m_entry) {
     return;
   }
-  wpi::SmallVector<double, 9> arr;
-  for (auto&& pose : m_poses) {
-    auto& translation = pose.Translation();
-    arr.push_back(translation.X().to<double>());
-    arr.push_back(translation.Y().to<double>());
-    arr.push_back(pose.Rotation().Degrees().to<double>());
-  }
-  if (setDefault) {
-    m_entry.SetDefaultDoubleArray(arr);
+  if (m_poses.size() < (255 / 3)) {
+    wpi::SmallVector<double, 9> arr;
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      arr.push_back(translation.X().to<double>());
+      arr.push_back(translation.Y().to<double>());
+      arr.push_back(pose.Rotation().Degrees().to<double>());
+    }
+    if (setDefault) {
+      m_entry.SetDefaultDoubleArray(arr);
+    } else {
+      m_entry.ForceSetDoubleArray(arr);
+    }
   } else {
-    m_entry.SetDoubleArray(arr);
+    // send as raw array of doubles if too big for NT array
+    std::vector<char> arr;
+    arr.resize(m_poses.size() * 3 * 8);
+    char* p = arr.data();
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.X().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.Y().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(pose.Rotation().Degrees().to<double>()));
+      p += 8;
+    }
+    if (setDefault) {
+      m_entry.SetDefaultRaw(wpi::StringRef{arr.data(), arr.size()});
+    } else {
+      m_entry.ForceSetRaw(wpi::StringRef{arr.data(), arr.size()});
+    }
   }
 }
 
@@ -85,18 +126,45 @@ void FieldObject2d::UpdateFromEntry() const {
     return;
   }
   auto val = m_entry.GetValue();
-  if (!val || !val->IsDoubleArray()) {
+  if (!val) {
     return;
   }
-  auto arr = val->GetDoubleArray();
-  auto size = arr.size();
-  if ((size % 3) != 0) {
-    return;
-  }
-  m_poses.resize(size / 3);
-  for (size_t i = 0; i < size / 3; ++i) {
-    m_poses[i] =
-        Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
-               Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+
+  if (val->IsDoubleArray()) {
+    auto arr = val->GetDoubleArray();
+    auto size = arr.size();
+    if ((size % 3) != 0) {
+      return;
+    }
+    m_poses.resize(size / 3);
+    for (size_t i = 0; i < size / 3; ++i) {
+      m_poses[i] =
+          Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+                 Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+    }
+  } else if (val->IsRaw()) {
+    // treat it simply as an array of doubles
+    wpi::StringRef data = val->GetRaw();
+
+    // must be triples of doubles
+    auto size = data.size();
+    if ((size % (3 * 8)) != 0) {
+      return;
+    }
+    m_poses.resize(size / (3 * 8));
+    const char* p = data.begin();
+    for (size_t i = 0; i < size / (3 * 8); ++i) {
+      double x = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double y = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double rot = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      m_poses[i] = Pose2d{units::meter_t{x}, units::meter_t{y},
+                          Rotation2d{units::degree_t{rot}}};
+    }
   }
 }
diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
index 1ac341412b..44b3f02339 100644
--- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h
+++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -23,6 +23,13 @@ class AnalogInput;
  */
 class AnalogEncoder : public Sendable, public SendableHelper<AnalogEncoder> {
  public:
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  explicit AnalogEncoder(int channel);
+
   /**
    * Construct a new AnalogEncoder attached to a specific AnalogInput.
    *
diff --git a/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
index a02fd4e445..b85eac51ed 100644
--- a/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -105,5 +105,7 @@ class HolonomicDriveController {
   frc2::PIDController m_xController;
   frc2::PIDController m_yController;
   ProfiledPIDController<units::radian> m_thetaController;
+
+  bool m_firstRun = true;
 };
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/romi/RomiGyro.h b/wpilibc/src/main/native/include/frc/romi/RomiGyro.h
deleted file mode 100644
index 7523448a0f..0000000000
--- a/wpilibc/src/main/native/include/frc/romi/RomiGyro.h
+++ /dev/null
@@ -1,105 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#pragma once
-
-#include <hal/SimDevice.h>
-
-#include "frc/interfaces/Gyro.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-/**
- * Use a rate gyro to return the robots heading relative to a starting position.
- *
- * This class is for the Romi onboard gyro, and will only work in
- * simulation/Romi mode. Only one instance of a RomiGyro is supported.
- */
-class RomiGyro : public Gyro, public Sendable, public SendableHelper<RomiGyro> {
- public:
-  RomiGyro();
-
-  /**
-   * Return the actual angle in degrees that the robot is currently facing.
-   *
-   * The angle is based on integration of the returned rate form the gyro.
-   * The angle is continuous, that is, it will continue from 360->361 degrees.
-   * This allows algorithms that wouldn't want to see a discontinuity in the
-   * gyro output as it sweeps from 360 to 0 on the second time around.
-   *
-   * @return the current heading of the robot in degrees.
-   */
-  double GetAngle() const override;
-
-  /**
-   * Return the rate of rotation of the gyro
-   *
-   * The rate is based on the most recent reading of the gyro.
-   *
-   * @return the current rate in degrees per second
-   */
-  double GetRate() const override;
-
-  /**
-   * Initialize the gyro.
-   *
-   * NOTE: This function is a no-op. The Romi gyro should be calibrated via
-   * the web UI.
-   */
-  void Calibrate() final {}
-
-  /**
-   * Gets the rate of turn in degrees-per-second around the X-axis
-   */
-  double GetRateX() const;
-
-  /**
-   * Gets the rate of turn in degrees-per-second around the Y-axis
-   */
-  double GetRateY() const;
-
-  /**
-   * Gets the rate of turn in degrees-per-second around the Z-axis
-   */
-  double GetRateZ() const;
-
-  /**
-   * Gets the currently reported angle around the X-axis
-   */
-  double GetAngleX() const;
-
-  /**
-   * Gets the currently reported angle around the X-axis
-   */
-  double GetAngleY() const;
-
-  /**
-   * Gets the currently reported angle around the X-axis
-   */
-  double GetAngleZ() const;
-
-  /**
-   * Resets the gyro
-   */
-  void Reset() override;
-
-  void InitSendable(SendableBuilder& builder) override;
-
- private:
-  hal::SimDevice m_simDevice;
-  hal::SimDouble m_simRateX;
-  hal::SimDouble m_simRateY;
-  hal::SimDouble m_simRateZ;
-  hal::SimDouble m_simAngleX;
-  hal::SimDouble m_simAngleY;
-  hal::SimDouble m_simAngleZ;
-
-  double m_angleXOffset = 0;
-  double m_angleYOffset = 0;
-  double m_angleZOffset = 0;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
index 4648074517..9f2272c661 100644
--- a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
@@ -18,7 +18,7 @@ namespace frc::sim {
 class FlywheelSim : public LinearSystemSim<1, 1, 1> {
  public:
   /**
-   * Creates a simulated flywhel mechanism.
+   * Creates a simulated flywheel mechanism.
    *
    * @param plant              The linear system representing the flywheel.
    * @param gearbox            The type of and number of motors in the flywheel
@@ -32,7 +32,7 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**
-   * Creates a simulated flywhel mechanism.
+   * Creates a simulated flywheel mechanism.
    *
    * @param gearbox            The type of and number of motors in the flywheel
    *                           gearbox.
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h
index 0319cbabd3..eb3625181c 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h
@@ -21,6 +21,7 @@
 namespace frc {
 
 class Field2d;
+class Trajectory;
 
 /**
  * Game field object on a Field2d.
@@ -76,6 +77,13 @@ class FieldObject2d {
    */
   void SetPoses(std::initializer_list<Pose2d> poses);
 
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which poses should be added.
+   */
+  void SetTrajectory(const Trajectory& trajectory);
+
   /**
    * Get multiple poses.
    *
diff --git a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
index d12eaf76cb..388c346c31 100644
--- a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -47,3 +47,17 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
   EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
                     kAngularTolerance);
 }
+
+TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+      frc::ProfiledPIDController<units::radian>{
+          1, 0, 0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              4_rad_per_s, 2_rad_per_s / 1_s}}};
+
+  frc::ChassisSpeeds speeds = controller.Calculate(
+      frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+
+  EXPECT_EQ(0, speeds.omega.to<double>());
+}
diff --git a/wpilibc/src/main/native/cpp/romi/RomiGyro.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp
similarity index 71%
rename from wpilibc/src/main/native/cpp/romi/RomiGyro.cpp
rename to wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp
index 26a28fac73..08a537bca3 100644
--- a/wpilibc/src/main/native/cpp/romi/RomiGyro.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp
@@ -2,11 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "frc/romi/RomiGyro.h"
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
+#include "sensors/RomiGyro.h"
 
 RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
   if (m_simDevice) {
@@ -26,15 +22,7 @@ RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
   }
 }
 
-double RomiGyro::GetAngle() const {
-  return GetAngleZ();
-}
-
-double RomiGyro::GetRate() const {
-  return GetRateZ();
-}
-
-double RomiGyro::GetRateX() const {
+double RomiGyro::GetRateX() {
   if (m_simRateX) {
     return m_simRateX.Get();
   }
@@ -42,7 +30,7 @@ double RomiGyro::GetRateX() const {
   return 0.0;
 }
 
-double RomiGyro::GetRateY() const {
+double RomiGyro::GetRateY() {
   if (m_simRateY) {
     return m_simRateY.Get();
   }
@@ -50,7 +38,7 @@ double RomiGyro::GetRateY() const {
   return 0.0;
 }
 
-double RomiGyro::GetRateZ() const {
+double RomiGyro::GetRateZ() {
   if (m_simRateZ) {
     return m_simRateZ.Get();
   }
@@ -58,7 +46,7 @@ double RomiGyro::GetRateZ() const {
   return 0.0;
 }
 
-double RomiGyro::GetAngleX() const {
+double RomiGyro::GetAngleX() {
   if (m_simAngleX) {
     return m_simAngleX.Get() - m_angleXOffset;
   }
@@ -66,7 +54,7 @@ double RomiGyro::GetAngleX() const {
   return 0.0;
 }
 
-double RomiGyro::GetAngleY() const {
+double RomiGyro::GetAngleY() {
   if (m_simAngleY) {
     return m_simAngleY.Get() - m_angleYOffset;
   }
@@ -74,7 +62,7 @@ double RomiGyro::GetAngleY() const {
   return 0.0;
 }
 
-double RomiGyro::GetAngleZ() const {
+double RomiGyro::GetAngleZ() {
   if (m_simAngleZ) {
     return m_simAngleZ.Get() - m_angleZOffset;
   }
@@ -89,9 +77,3 @@ void RomiGyro::Reset() {
     m_angleZOffset = m_simAngleZ.Get();
   }
 }
-
-void RomiGyro::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Gyro");
-  builder.AddDoubleProperty(
-      "Value", [=]() { return GetAngle(); }, nullptr);
-}
diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h
new file mode 100644
index 0000000000..0e93d488ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+class RomiGyro {
+ public:
+  RomiGyro();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the X-axis
+   */
+  double GetRateX();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Y-axis
+   */
+  double GetRateY();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Z-axis
+   */
+  double GetRateZ();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleX();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleY();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleZ();
+
+  /**
+   * Resets the gyro
+   */
+  void Reset();
+
+ private:
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simRateX;
+  hal::SimDouble m_simRateY;
+  hal::SimDouble m_simRateZ;
+  hal::SimDouble m_simAngleX;
+  hal::SimDouble m_simAngleY;
+  hal::SimDouble m_simAngleZ;
+
+  double m_angleXOffset = 0;
+  double m_angleYOffset = 0;
+  double m_angleZOffset = 0;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
index ace7d33bc7..98ae9574b0 100644
--- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
@@ -8,10 +8,11 @@
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/motorcontrol/Spark.h>
-#include <frc/romi/RomiGyro.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/length.h>
 
+#include "sensors/RomiGyro.h"
+
 class Drivetrain : public frc2::SubsystemBase {
  public:
   static constexpr double kCountsPerRevolution = 1440.0;
@@ -116,6 +117,6 @@ class Drivetrain : public frc2::SubsystemBase {
 
   frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
 
-  frc::RomiGyro m_gyro;
+  RomiGyro m_gyro;
   frc::BuiltInAccelerometer m_accelerometer;
 };
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
index dbbe72bd27..c5dc5a2946 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
@@ -23,6 +23,15 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
   protected SimDevice m_simDevice;
   protected SimDouble m_simPosition;
 
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  public AnalogEncoder(int channel) {
+    this(new AnalogInput(channel));
+  }
+
   /**
    * Construct a new AnalogEncoder attached to a specific AnalogInput.
    *
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
index 77901fcfb1..93f0977223 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -205,15 +205,8 @@ public class Compressor implements Sendable, AutoCloseable {
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Compressor");
     builder.addBooleanProperty(
-        "Enabled",
-        this::enabled,
-        value -> {
-          if (value) {
-            start();
-          } else {
-            stop();
-          }
-        });
+        "Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl);
+    builder.addBooleanProperty("Enabled", this::enabled, null);
     builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
   }
 }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
index 0072d36157..f1d7f81da7 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
@@ -31,6 +31,8 @@ public class HolonomicDriveController {
   private final PIDController m_yController;
   private final ProfiledPIDController m_thetaController;
 
+  private boolean m_firstRun = true;
+
   /**
    * Constructs a holonomic drive controller.
    *
@@ -82,6 +84,13 @@ public class HolonomicDriveController {
   @SuppressWarnings("LocalVariableName")
   public ChassisSpeeds calculate(
       Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
+    // If this is the first run, then we need to reset the theta controller to the current pose's
+    // heading.
+    if (m_firstRun) {
+      m_thetaController.reset(currentPose.getRotation().getRadians());
+      m_firstRun = false;
+    }
+
     // Calculate feedforward velocities (field-relative).
     double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
     double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
index d9054e929a..aee7e13dac 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
@@ -8,6 +8,9 @@ import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -82,6 +85,19 @@ public class FieldObject2d {
     updateEntry();
   }
 
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which the poses should be added.
+   */
+  public synchronized void setTrajectory(Trajectory trajectory) {
+    m_poses.clear();
+    for (Trajectory.State state : trajectory.getStates()) {
+      m_poses.add(state.poseMeters);
+    }
+    updateEntry();
+  }
+
   /**
    * Get multiple poses.
    *
@@ -101,20 +117,39 @@ public class FieldObject2d {
       return;
     }
 
-    double[] arr = new double[m_poses.size() * 3];
-    int ndx = 0;
-    for (Pose2d pose : m_poses) {
-      Translation2d translation = pose.getTranslation();
-      arr[ndx + 0] = translation.getX();
-      arr[ndx + 1] = translation.getY();
-      arr[ndx + 2] = pose.getRotation().getDegrees();
-      ndx += 3;
-    }
+    if (m_poses.size() < (255 / 3)) {
+      double[] arr = new double[m_poses.size() * 3];
+      int ndx = 0;
+      for (Pose2d pose : m_poses) {
+        Translation2d translation = pose.getTranslation();
+        arr[ndx + 0] = translation.getX();
+        arr[ndx + 1] = translation.getY();
+        arr[ndx + 2] = pose.getRotation().getDegrees();
+        ndx += 3;
+      }
 
-    if (setDefault) {
-      m_entry.setDefaultDoubleArray(arr);
+      if (setDefault) {
+        m_entry.setDefaultDoubleArray(arr);
+      } else {
+        m_entry.setDoubleArray(arr);
+      }
     } else {
-      m_entry.setDoubleArray(arr);
+      // send as raw array of doubles if too big for NT array
+      ByteBuffer output = ByteBuffer.allocate(m_poses.size() * 3 * 8);
+      output.order(ByteOrder.BIG_ENDIAN);
+
+      for (Pose2d pose : m_poses) {
+        Translation2d translation = pose.getTranslation();
+        output.putDouble(translation.getX());
+        output.putDouble(translation.getY());
+        output.putDouble(pose.getRotation().getDegrees());
+      }
+
+      if (setDefault) {
+        m_entry.setDefaultRaw(output.array());
+      } else {
+        m_entry.forceSetRaw(output.array());
+      }
     }
   }
 
@@ -125,17 +160,36 @@ public class FieldObject2d {
     }
 
     double[] arr = m_entry.getDoubleArray((double[]) null);
-    if (arr == null) {
-      return;
-    }
+    if (arr != null) {
+      if ((arr.length % 3) != 0) {
+        return;
+      }
 
-    if ((arr.length % 3) != 0) {
-      return;
-    }
+      m_poses.clear();
+      for (int i = 0; i < arr.length; i += 3) {
+        m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
+      }
+    } else {
+      // read as raw array of doubles
+      byte[] data = m_entry.getRaw((byte[]) null);
+      if (data == null) {
+        return;
+      }
 
-    m_poses.clear();
-    for (int i = 0; i < arr.length; i += 3) {
-      m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
+      // must be triples of doubles
+      if ((data.length % (3 * 8)) != 0) {
+        return;
+      }
+      ByteBuffer input = ByteBuffer.wrap(data);
+      input.order(ByteOrder.BIG_ENDIAN);
+
+      m_poses.clear();
+      for (int i = 0; i < (data.length / (3 * 8)); i++) {
+        double x = input.getDouble();
+        double y = input.getDouble();
+        double rot = input.getDouble();
+        m_poses.add(new Pose2d(x, y, Rotation2d.fromDegrees(rot)));
+      }
     }
   }
 
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
index e63b6b1c25..c7610b8329 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
@@ -72,4 +72,19 @@ class HolonomicDriveControllerTest {
                 MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
                 kAngularTolerance));
   }
+
+  @Test
+  void testDoesNotRotateUnnecessarily() {
+    var controller =
+        new HolonomicDriveController(
+            new PIDController(1, 0, 0),
+            new PIDController(1, 0, 0),
+            new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
+
+    ChassisSpeeds speeds =
+        controller.calculate(
+            new Pose2d(0, 0, new Rotation2d(1.57)), new Pose2d(), 0, new Rotation2d(1.57));
+
+    assertEquals(0.0, speeds.omegaRadiansPerSecond);
+  }
 }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
similarity index 64%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java
rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
index 3c5e228606..c21f62317e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
@@ -2,17 +2,13 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package edu.wpi.first.wpilibj.romi;
+package edu.wpi.first.wpilibj.examples.romireference.sensors;
 
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 
-public class RomiGyro implements Gyro, Sendable {
-  private final SimDevice m_simDevice;
+public class RomiGyro {
   private SimDouble m_simRateX;
   private SimDouble m_simRateY;
   private SimDouble m_simRateZ;
@@ -26,16 +22,16 @@ public class RomiGyro implements Gyro, Sendable {
 
   /** Create a new RomiGyro. */
   public RomiGyro() {
-    m_simDevice = SimDevice.create("Gyro:RomiGyro");
-    if (m_simDevice != null) {
-      m_simDevice.createBoolean("init", Direction.kOutput, true);
-      m_simRateX = m_simDevice.createDouble("rate_x", Direction.kInput, 0.0);
-      m_simRateY = m_simDevice.createDouble("rate_y", Direction.kInput, 0.0);
-      m_simRateZ = m_simDevice.createDouble("rate_z", Direction.kInput, 0.0);
+    SimDevice gyroSimDevice = SimDevice.create("Gyro:RomiGyro");
+    if (gyroSimDevice != null) {
+      gyroSimDevice.createBoolean("init", Direction.kOutput, true);
+      m_simRateX = gyroSimDevice.createDouble("rate_x", Direction.kInput, 0.0);
+      m_simRateY = gyroSimDevice.createDouble("rate_y", Direction.kInput, 0.0);
+      m_simRateZ = gyroSimDevice.createDouble("rate_z", Direction.kInput, 0.0);
 
-      m_simAngleX = m_simDevice.createDouble("angle_x", Direction.kInput, 0.0);
-      m_simAngleY = m_simDevice.createDouble("angle_y", Direction.kInput, 0.0);
-      m_simAngleZ = m_simDevice.createDouble("angle_z", Direction.kInput, 0.0);
+      m_simAngleX = gyroSimDevice.createDouble("angle_x", Direction.kInput, 0.0);
+      m_simAngleY = gyroSimDevice.createDouble("angle_y", Direction.kInput, 0.0);
+      m_simAngleZ = gyroSimDevice.createDouble("angle_z", Direction.kInput, 0.0);
     }
   }
 
@@ -118,7 +114,6 @@ public class RomiGyro implements Gyro, Sendable {
   }
 
   /** Reset the gyro angles to 0. */
-  @Override
   public void reset() {
     if (m_simAngleX != null) {
       m_angleXOffset = m_simAngleX.get();
@@ -126,32 +121,4 @@ public class RomiGyro implements Gyro, Sendable {
       m_angleZOffset = m_simAngleZ.get();
     }
   }
-
-  @Override
-  public void calibrate() {
-    // no-op - Romi Gyro calibration is done via web UI
-  }
-
-  @Override
-  public double getAngle() {
-    return getAngleZ();
-  }
-
-  @Override
-  public double getRate() {
-    return getRateZ();
-  }
-
-  @Override
-  public void close() throws Exception {
-    if (m_simDevice != null) {
-      m_simDevice.close();
-    }
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Gyro");
-    builder.addDoubleProperty("Value", this::getAngle, null);
-  }
 }
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
index cc95e0475c..35ea1a511d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
@@ -7,8 +7,8 @@ package edu.wpi.first.wpilibj.examples.romireference.subsystems;
 import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.romireference.sensors.RomiGyro;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.romi.RomiGyro;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index e0cc426841..a22ac0abfa 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -91,6 +91,38 @@ public final class WPIMathJNI {
    */
   public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
 
+  /**
+   * Loads a Pathweaver JSON.
+   *
+   * @param path The path to the JSON.
+   * @return A double array with the trajectory states from the JSON.
+   */
+  public static native double[] fromPathweaverJson(String path) throws IOException;
+
+  /**
+   * Converts a trajectory into a Pathweaver JSON and saves it.
+   *
+   * @param elements The elements of the trajectory.
+   * @param path The location to save the JSON to.
+   */
+  public static native void toPathweaverJson(double[] elements, String path) throws IOException;
+
+  /**
+   * Deserializes a trajectory JSON into a double[] of trajectory elements.
+   *
+   * @param json The JSON containing the serialized trajectory.
+   * @return A double array with the trajectory states.
+   */
+  public static native double[] deserializeTrajectory(String json);
+
+  /**
+   * Serializes the trajectory into a JSON string.
+   *
+   * @param elements The elements of the trajectory.
+   * @return A JSON containing the serialized trajectory.
+   */
+  public static native String serializeTrajectory(double[] elements);
+
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
index 5d0e649e59..1f782d5281 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
@@ -74,6 +74,9 @@ public class LinearFilter {
    * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
    * gain = e^(-dt / T), T is the time constant in seconds.
    *
+   * <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency above which the
+   * input starts to attenuate.
+   *
    * <p>This filter is stable for time constants greater than zero.
    *
    * @param timeConstant The discrete-time time constant in seconds.
@@ -91,6 +94,9 @@ public class LinearFilter {
    * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
    * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
    *
+   * <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency below which the
+   * input starts to attenuate.
+   *
    * <p>This filter is stable for time constants greater than zero.
    *
    * @param timeConstant The discrete-time time constant in seconds.
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
index 1e64bfd95c..44b549723e 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
@@ -80,6 +80,26 @@ public class CubicHermiteSpline extends Spline {
    */
   private SimpleMatrix makeHermiteBasis() {
     if (hermiteBasis == null) {
+      // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
+      // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+      //
+      // P(i)    = P(0)  = a0
+      // P'(i)   = P'(0) = a1
+      // P(i+1)  = P(1)  = a3 + a2 + a1 + a0
+      // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+      //
+      // [ P(i)    ] = [ 0 0 0 1 ][ a3 ]
+      // [ P'(i)   ] = [ 0 0 1 0 ][ a2 ]
+      // [ P(i+1)  ] = [ 1 1 1 1 ][ a1 ]
+      // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+      //
+      // To solve for the coefficients, we can invert the 4x4 matrix and move it
+      // to the other side of the equation.
+      //
+      // [ a3 ] = [  2  1 -2  1 ][ P(i)    ]
+      // [ a2 ] = [ -3 -2  3 -1 ][ P'(i)   ]
+      // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
+      // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
       hermiteBasis =
           new SimpleMatrix(
               4,
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
index 4f832103b0..d189a4fdf4 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
@@ -80,13 +80,40 @@ public class QuinticHermiteSpline extends Spline {
    */
   private SimpleMatrix makeHermiteBasis() {
     if (hermiteBasis == null) {
+      // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
+      // vectors, we want to find the coefficients of the spline
+      // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+      //
+      // P(i)     = P(0)   = a0
+      // P'(i)    = P'(0)  = a1
+      // P''(i)   = P''(0) = 2 * a2
+      // P(i+1)   = P(1)   = a5 + a4 + a3 + a2 + a1 + a0
+      // P'(i+1)  = P'(1)  = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
+      // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+      //
+      // [ P(i)     ] = [  0  0  0  0  0  1 ][ a5 ]
+      // [ P'(i)    ] = [  0  0  0  0  1  0 ][ a4 ]
+      // [ P''(i)   ] = [  0  0  0  2  0  0 ][ a3 ]
+      // [ P(i+1)   ] = [  1  1  1  1  1  1 ][ a2 ]
+      // [ P'(i+1)  ] = [  5  4  3  2  1  0 ][ a1 ]
+      // [ P''(i+1) ] = [ 20 12  6  2  0  0 ][ a0 ]
+      //
+      // To solve for the coefficients, we can invert the 6x6 matrix and move it
+      // to the other side of the equation.
+      //
+      // [ a5 ] = [  -6.0  -3.0  -0.5   6.0  -3.0   0.5 ][ P(i)     ]
+      // [ a4 ] = [  15.0   8.0   1.5 -15.0   7.0  -1.0 ][ P'(i)    ]
+      // [ a3 ] = [ -10.0  -6.0  -1.5  10.0  -4.0   0.5 ][ P''(i)   ]
+      // [ a2 ] = [   0.0   0.0   0.5   0.0   0.0   0.0 ][ P(i+1)   ]
+      // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
+      // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
       hermiteBasis =
           new SimpleMatrix(
               6,
               6,
               true,
               new double[] {
-                -06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+                -06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
                 -10.0, -06.0, -01.5, +10.0, -04.0, +00.5, +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
                 +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
               });
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
index 60c4a49013..b9b14fd826 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
@@ -4,73 +4,117 @@
 
 package edu.wpi.first.wpilibj.trajectory;
 
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import com.fasterxml.jackson.databind.ObjectReader;
-import com.fasterxml.jackson.databind.ObjectWriter;
-import java.io.BufferedReader;
-import java.io.BufferedWriter;
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import java.io.IOException;
-import java.nio.file.Files;
 import java.nio.file.Path;
-import java.util.Arrays;
+import java.util.ArrayList;
+import java.util.List;
 
 public final class TrajectoryUtil {
-  private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class);
-  private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class);
-
   private TrajectoryUtil() {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
+  /**
+   * Creates a trajectory from a double[] of elements.
+   *
+   * @param elements A double[] containing the raw elements of the trajectory.
+   * @return A trajectory created from the elements.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  private static Trajectory createTrajectoryFromElements(double[] elements) {
+    // Make sure that the elements have the correct length.
+    if (elements.length % 7 != 0) {
+      throw new TrajectorySerializationException(
+          "An error occurred when converting trajectory elements into a trajectory.");
+    }
+
+    // Create a list of states from the elements.
+    List<Trajectory.State> states = new ArrayList<>();
+    for (int i = 0; i < elements.length; i += 7) {
+      states.add(
+          new Trajectory.State(
+              elements[i],
+              elements[i + 1],
+              elements[i + 2],
+              new Pose2d(elements[i + 3], elements[i + 4], new Rotation2d(elements[i + 5])),
+              elements[i + 6]));
+    }
+    return new Trajectory(states);
+  }
+
+  /**
+   * Returns a double[] of elements from the given trajectory.
+   *
+   * @param trajectory The trajectory to retrieve raw elements from.
+   * @return A double[] of elements from the given trajectory.
+   */
+  private static double[] getElementsFromTrajectory(Trajectory trajectory) {
+    // Create a double[] of elements and fill it from the trajectory states.
+    double[] elements = new double[trajectory.getStates().size() * 7];
+
+    for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) {
+      var state = trajectory.getStates().get(i / 7);
+      elements[i] = state.timeSeconds;
+      elements[i + 1] = state.velocityMetersPerSecond;
+      elements[i + 2] = state.accelerationMetersPerSecondSq;
+      elements[i + 3] = state.poseMeters.getX();
+      elements[i + 4] = state.poseMeters.getY();
+      elements[i + 5] = state.poseMeters.getRotation().getRadians();
+      elements[i + 6] = state.curvatureRadPerMeter;
+    }
+    return elements;
+  }
+
   /**
    * Imports a Trajectory from a PathWeaver-style JSON file.
    *
-   * @param path the path of the json file to import from
+   * @param path The path of the json file to import from
    * @return The trajectory represented by the file.
-   * @throws IOException if reading from the file fails
+   * @throws IOException if reading from the file fails.
    */
   public static Trajectory fromPathweaverJson(Path path) throws IOException {
-    try (BufferedReader reader = Files.newBufferedReader(path)) {
-      Trajectory.State[] state = READER.readValue(reader);
-      return new Trajectory(Arrays.asList(state));
-    }
+    return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
   }
 
   /**
    * Exports a Trajectory to a PathWeaver-style JSON file.
    *
-   * @param trajectory the trajectory to export
-   * @param path the path of the file to export to
-   * @throws IOException if writing to the file fails
+   * @param trajectory The trajectory to export
+   * @param path The path of the file to export to
+   * @throws IOException if writing to the file fails.
    */
   public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
-    Files.createDirectories(path.getParent());
-    try (BufferedWriter writer = Files.newBufferedWriter(path)) {
-      WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
-    }
+    WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
   }
 
   /**
    * Deserializes a Trajectory from PathWeaver-style JSON.
    *
-   * @param json the string containing the serialized JSON
+   * @param json The string containing the serialized JSON
    * @return the trajectory represented by the JSON
-   * @throws JsonProcessingException if deserializing the JSON fails
+   * @throws TrajectorySerializationException if deserialization of the string fails.
    */
-  public static Trajectory deserializeTrajectory(String json) throws JsonProcessingException {
-    Trajectory.State[] state = READER.readValue(json);
-    return new Trajectory(Arrays.asList(state));
+  public static Trajectory deserializeTrajectory(String json) {
+    return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
   }
 
   /**
    * Serializes a Trajectory to PathWeaver-style JSON.
    *
-   * @param trajectory the trajectory to export
-   * @return the string containing the serialized JSON
-   * @throws JsonProcessingException if serializing the Trajectory fails
+   * @param trajectory The trajectory to export
+   * @return The string containing the serialized JSON
+   * @throws TrajectorySerializationException if serialization of the trajectory fails.
    */
-  public static String serializeTrajectory(Trajectory trajectory) throws JsonProcessingException {
-    return WRITER.writeValueAsString(trajectory.getStates().toArray(new Trajectory.State[0]));
+  public static String serializeTrajectory(Trajectory trajectory) {
+    return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
+  }
+
+  public static class TrajectorySerializationException extends RuntimeException {
+    public TrajectorySerializationException(String message) {
+      super(message);
+    }
   }
 }
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index 18d887b63a..52e68679e5 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -13,6 +13,7 @@
 #include "Eigen/QR"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
 #include "unsupported/Eigen/MatrixFunctions"
 
 using namespace wpi::java;
@@ -43,6 +44,46 @@ bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
   return true;
 }
 
+std::vector<double> GetElementsFromTrajectory(
+    const frc::Trajectory& trajectory) {
+  std::vector<double> elements;
+  elements.reserve(trajectory.States().size() * 7);
+
+  for (auto&& state : trajectory.States()) {
+    elements.push_back(state.t.to<double>());
+    elements.push_back(state.velocity.to<double>());
+    elements.push_back(state.acceleration.to<double>());
+    elements.push_back(state.pose.X().to<double>());
+    elements.push_back(state.pose.Y().to<double>());
+    elements.push_back(state.pose.Rotation().Radians().to<double>());
+    elements.push_back(state.curvature.to<double>());
+  }
+
+  return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(wpi::ArrayRef<double> elements) {
+  // Make sure that the elements have the correct length.
+  assert(elements.size() % 7 == 0);
+
+  // Create a vector of states from the elements.
+  std::vector<frc::Trajectory::State> states;
+  states.reserve(elements.size() / 7);
+
+  for (size_t i = 0; i < elements.size(); i += 7) {
+    states.emplace_back(frc::Trajectory::State{
+        units::second_t{elements[i]},
+        units::meters_per_second_t{elements[i + 1]},
+        units::meters_per_second_squared_t{elements[i + 2]},
+        frc::Pose2d{units::meter_t{elements[i + 3]},
+                    units::meter_t{elements[i + 4]},
+                    units::radian_t{elements[i + 5]}},
+        units::curvature_t{elements[i + 6]}});
+  }
+
+  return frc::Trajectory(states);
+}
+
 extern "C" {
 
 /*
@@ -163,4 +204,99 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
   return isStabilizable;
 }
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+  (JNIEnv* env, jclass, jstring path)
+{
+  try {
+    auto trajectory =
+        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass("java/lang/IOException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
+    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+                                          JStringRef{env, path}.c_str());
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass("java/lang/IOException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+  (JNIEnv* env, jclass, jstring json)
+{
+  try {
+    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+        JStringRef{env, json}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass(
+        "edu/wpi/first/wpilibj/trajectory/TrajectoryUtil$"
+        "TrajectorySerializationException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+  (JNIEnv* env, jclass, jdoubleArray elements)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
+    return MakeJString(env,
+                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass(
+        "edu/wpi/first/wpilibj/trajectory/TrajectoryUtil$"
+        "TrajectorySerializationException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
 }  // extern "C"
diff --git a/wpimath/src/main/native/include/frc/LinearFilter.h b/wpimath/src/main/native/include/frc/LinearFilter.h
index 3faec05f8f..449fadf196 100644
--- a/wpimath/src/main/native/include/frc/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/LinearFilter.h
@@ -104,6 +104,9 @@ class LinearFilter {
    *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
    * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
    *
+   * Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
+   * above which the input starts to attenuate.
+   *
    * This filter is stable for time constants greater than zero.
    *
    * @param timeConstant The discrete-time time constant in seconds.
@@ -121,6 +124,9 @@ class LinearFilter {
    *   y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
    * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
    *
+   * Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
+   * below which the input starts to attenuate.
+   *
    * This filter is stable for time constants greater than zero.
    *
    * @param timeConstant The discrete-time time constant in seconds.
diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index c30a8a89a8..b908bb2e50 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -52,6 +52,27 @@ class CubicHermiteSpline : public Spline<3> {
    * @return The hermite basis matrix for cubic hermite spline interpolation.
    */
   static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+    // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
+    // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+    //
+    // P(i)    = P(0)  = a0
+    // P'(i)   = P'(0) = a1
+    // P(i+1)  = P(1)  = a3 + a2 + a1 + a0
+    // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+    //
+    // [ P(i)    ] = [ 0 0 0 1 ][ a3 ]
+    // [ P'(i)   ] = [ 0 0 1 0 ][ a2 ]
+    // [ P(i+1)  ] = [ 1 1 1 1 ][ a1 ]
+    // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+    //
+    // To solve for the coefficients, we can invert the 4x4 matrix and move it
+    // to the other side of the equation.
+    //
+    // [ a3 ] = [  2  1 -2  1 ][ P(i)    ]
+    // [ a2 ] = [ -3 -2  3 -1 ][ P'(i)   ]
+    // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
+    // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
+
     // clang-format off
     static auto basis = (Eigen::Matrix<double, 4, 4>() <<
      +2.0, +1.0, -2.0, +1.0,
diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index ff9369ce27..05df2fe432 100644
--- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -52,10 +52,38 @@ class QuinticHermiteSpline : public Spline<5> {
    * @return The hermite basis matrix for quintic hermite spline interpolation.
    */
   static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
+    // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
+    // vectors, we want to find the coefficients of the spline
+    // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+    //
+    // P(i)     = P(0)   = a0
+    // P'(i)    = P'(0)  = a1
+    // P''(i)   = P''(0) = 2 * a2
+    // P(i+1)   = P(1)   = a5 + a4 + a3 + a2 + a1 + a0
+    // P'(i+1)  = P'(1)  = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
+    // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+    //
+    // [ P(i)     ] = [  0  0  0  0  0  1 ][ a5 ]
+    // [ P'(i)    ] = [  0  0  0  0  1  0 ][ a4 ]
+    // [ P''(i)   ] = [  0  0  0  2  0  0 ][ a3 ]
+    // [ P(i+1)   ] = [  1  1  1  1  1  1 ][ a2 ]
+    // [ P'(i+1)  ] = [  5  4  3  2  1  0 ][ a1 ]
+    // [ P''(i+1) ] = [ 20 12  6  2  0  0 ][ a0 ]
+    //
+    // To solve for the coefficients, we can invert the 6x6 matrix and move it
+    // to the other side of the equation.
+    //
+    // [ a5 ] = [  -6.0  -3.0  -0.5   6.0  -3.0   0.5 ][ P(i)     ]
+    // [ a4 ] = [  15.0   8.0   1.5 -15.0   7.0  -1.0 ][ P'(i)    ]
+    // [ a3 ] = [ -10.0  -6.0  -1.5  10.0  -4.0   0.5 ][ P''(i)   ]
+    // [ a2 ] = [   0.0   0.0   0.5   0.0   0.0   0.0 ][ P(i+1)   ]
+    // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
+    // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
+
     // clang-format off
     static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
       -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
-      +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+      +15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
       -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
       +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
       +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
index 297c5703e8..69482b8f73 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -36,19 +36,19 @@ class TrajectoryUtil {
   static Trajectory FromPathweaverJson(const wpi::Twine& path);
 
   /**
-     * Deserializes a Trajectory from PathWeaver-style JSON.
-
-     * @param json the string containing the serialized JSON
-
-     * @return the trajectory represented by the JSON
-     */
+   * Deserializes a Trajectory from PathWeaver-style JSON.
+   *
+   * @param json the string containing the serialized JSON
+   *
+   * @return the trajectory represented by the JSON
+   */
   static std::string SerializeTrajectory(const Trajectory& trajectory);
 
   /**
    * Serializes a Trajectory to PathWeaver-style JSON.
-
+   *
    * @param trajectory the trajectory to export
-
+   *
    * @return the string containing the serialized JSON
    */
   static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
diff --git a/wpiutil/src/main/native/include/wpi/circular_buffer.inc b/wpiutil/src/main/native/include/wpi/circular_buffer.inc
index 0b8aca5f13..14667fc38d 100644
--- a/wpiutil/src/main/native/include/wpi/circular_buffer.inc
+++ b/wpiutil/src/main/native/include/wpi/circular_buffer.inc
@@ -11,7 +11,7 @@
 namespace wpi {
 
 template <class T>
-circular_buffer<T>::circular_buffer(size_t size) : m_data(size, T{0}) {}
+circular_buffer<T>::circular_buffer(size_t size) : m_data(size, T{}) {}
 
 /**
  * Returns number of elements in buffer
diff --git a/wpiutil/src/main/native/unix/StackTrace.cpp b/wpiutil/src/main/native/unix/StackTrace.cpp
index f6ebde380b..3b14925e56 100644
--- a/wpiutil/src/main/native/unix/StackTrace.cpp
+++ b/wpiutil/src/main/native/unix/StackTrace.cpp
@@ -26,8 +26,11 @@ std::string GetStackTrace(int offset) {
       // extract just function name from "pathToExe(functionName+offset)"
       StringRef sym{mangledSymbols[i]};
       sym = sym.split('(').second;
-      sym = sym.split('+').first;
-      trace << "\tat " << Demangle(sym) << "\n";
+      StringRef offset;
+      std::tie(sym, offset) = sym.split('+');
+      StringRef addr;
+      std::tie(offset, addr) = offset.split(')');
+      trace << "\tat " << Demangle(sym) << " + " << offset << addr << "\n";
     }
   }