[commands] Subsystem: Add default command removal method (#5064)

HAL_GetRuntimeType used to be a free call before the roboRIO2 was added. However, nLoadOut::getTargetClass() is not a free call, and it may hit the IPC layer. Cache this value so it is not called every time.

.clang-format

@@ -156,7 +156,7 @@ SpacesInCStyleCastParentheses: false
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
 SpaceBeforeSquareBrackets: false
-Standard:        c++17
+Standard:        c++20
 StatementMacros:
   - Q_UNUSED
   - QT_REQUIRE_VERSION

docs/build.gradle

@@ -64,6 +64,7 @@ doxygen {
             cppIncludeRoots.add(it.absolutePath)
         }
     }
+    cppIncludeRoots << '../ntcore/build/generated/main/native/include/'
 
     if (project.hasProperty('docWarningsAsErrors')) {
         // C++20 shims

glass/src/libnt/native/cpp/NetworkTables.cpp

@@ -400,6 +400,11 @@ void NetworkTablesModel::ValueSource::UpdateFromValue(
         }
       } else {
         valueChildren.clear();
+        valueStr.clear();
+        wpi::raw_string_ostream os{valueStr};
+        os << '"';
+        os.write_escaped(value.GetString());
+        os << '"';
       }
       break;
     case NT_RAW:
@@ -769,25 +774,32 @@ static bool StringToStringArray(std::string_view in,
   }
   in = wpi::trim(in);
 
-  wpi::SmallVector<std::string_view, 16> inSplit;
-  wpi::SmallString<32> buf;
-
-  wpi::split(in, inSplit, ',', -1, false);
-  for (auto val : inSplit) {
-    val = wpi::trim(val);
-    if (val.empty()) {
-      continue;
-    }
-    if (val.front() != '"' || val.back() != '"') {
-      fmt::print(stderr,
-                 "GUI: NetworkTables: Could not understand value '{}'\n", val);
+  while (!in.empty()) {
+    if (in.front() != '"') {
+      fmt::print(stderr, "GUI: NetworkTables: Expected '\"'");
       return false;
     }
-    val.remove_prefix(1);
-    val.remove_suffix(1);
-    out->emplace_back(wpi::UnescapeCString(val, buf).first);
+    in.remove_prefix(1);
+    wpi::SmallString<128> buf;
+    std::string_view val;
+    std::tie(val, in) = wpi::UnescapeCString(in, buf);
+    out->emplace_back(val);
+    if (!in.empty()) {
+      if (in.front() != '"') {
+        fmt::print(stderr, "GUI: NetworkTables: Error escaping string");
+        return false;
+      }
+      in.remove_prefix(1);
+      in = wpi::ltrim(in);
+    }
+    if (!in.empty()) {
+      if (in.front() != ',') {
+        fmt::print(stderr, "GUI: NetworkTables: Expected ','");
+        return false;
+      }
+      in.remove_prefix(1);
+    }
   }
-
   return true;
 }
 
@@ -826,9 +838,8 @@ static void EmitEntryValueReadonly(const NetworkTablesModel::ValueSource& entry,
       break;
     }
     case NT_STRING: {
-      // GetString() comes from a std::string, so it's null terminated
       ImGui::LabelText(typeStr ? typeStr : "string", "%s",
-                       val.GetString().data());
+                       entry.valueStr.c_str());
       break;
     }
     case NT_BOOLEAN_ARRAY:
@@ -938,13 +949,18 @@ static void EmitEntryValueEditable(NetworkTablesModel::Entry& entry,
       break;
     }
     case NT_STRING: {
-      char* v = GetTextBuffer(val.GetString());
+      char* v = GetTextBuffer(entry.valueStr);
       if (ImGui::InputText(typeStr ? typeStr : "string", v, kTextBufferSize,
                            ImGuiInputTextFlags_EnterReturnsTrue)) {
-        if (entry.publisher == 0) {
-          entry.publisher = nt::Publish(entry.info.topic, NT_STRING, "string");
+        if (v[0] == '"') {
+          if (entry.publisher == 0) {
+            entry.publisher =
+                nt::Publish(entry.info.topic, NT_STRING, "string");
+          }
+          wpi::SmallString<128> buf;
+          nt::SetString(entry.publisher,
+                        wpi::UnescapeCString(v + 1, buf).first);
         }
-        nt::SetString(entry.publisher, v);
       }
       break;
     }
@@ -1045,58 +1061,97 @@ static void CreateTopicMenuItem(NetworkTablesModel* model,
         model->AddEntry(nt::GetTopic(model->GetInstance().GetHandle(), path));
     if (entry->publisher == 0) {
       entry->publisher = nt::Publish(entry->info.topic, type, typeStr);
+      // publish a default value so it's editable
+      switch (type) {
+        case NT_BOOLEAN:
+          nt::SetDefaultBoolean(entry->publisher, false);
+          break;
+        case NT_INTEGER:
+          nt::SetDefaultInteger(entry->publisher, 0);
+          break;
+        case NT_FLOAT:
+          nt::SetDefaultFloat(entry->publisher, 0.0);
+          break;
+        case NT_DOUBLE:
+          nt::SetDefaultDouble(entry->publisher, 0.0);
+          break;
+        case NT_STRING:
+          nt::SetDefaultString(entry->publisher, "");
+          break;
+        case NT_BOOLEAN_ARRAY:
+          nt::SetDefaultBooleanArray(entry->publisher, {});
+          break;
+        case NT_INTEGER_ARRAY:
+          nt::SetDefaultIntegerArray(entry->publisher, {});
+          break;
+        case NT_FLOAT_ARRAY:
+          nt::SetDefaultFloatArray(entry->publisher, {});
+          break;
+        case NT_DOUBLE_ARRAY:
+          nt::SetDefaultDoubleArray(entry->publisher, {});
+          break;
+        case NT_STRING_ARRAY:
+          nt::SetDefaultStringArray(entry->publisher, {});
+          break;
+        default:
+          break;
+      }
     }
   }
 }
 
+void glass::DisplayNetworkTablesAddMenu(NetworkTablesModel* model,
+                                        std::string_view path,
+                                        NetworkTablesFlags flags) {
+  static char nameBuffer[kTextBufferSize];
+
+  if (ImGui::BeginMenu("Add new...")) {
+    if (ImGui::IsWindowAppearing()) {
+      nameBuffer[0] = '\0';
+    }
+
+    ImGui::InputTextWithHint("New item name", "example", nameBuffer,
+                             kTextBufferSize);
+    std::string fullNewPath;
+    if (path == "/") {
+      path = "";
+    }
+    fullNewPath = fmt::format("{}/{}", path, nameBuffer);
+
+    ImGui::Text("Adding: %s", fullNewPath.c_str());
+    ImGui::Separator();
+    auto entry = model->GetEntry(fullNewPath);
+    bool exists = entry && entry->info.type != NT_Type::NT_UNASSIGNED;
+    bool enabled = (flags & NetworkTablesFlags_CreateNoncanonicalKeys ||
+                    nameBuffer[0] != '\0') &&
+                   !exists;
+
+    CreateTopicMenuItem(model, fullNewPath, NT_STRING, "string", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_INTEGER, "int", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_FLOAT, "float", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_DOUBLE, "double", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_BOOLEAN, "boolean", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_STRING_ARRAY, "string[]",
+                        enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_INTEGER_ARRAY, "int[]", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_FLOAT_ARRAY, "float[]", enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_DOUBLE_ARRAY, "double[]",
+                        enabled);
+    CreateTopicMenuItem(model, fullNewPath, NT_BOOLEAN_ARRAY, "boolean[]",
+                        enabled);
+
+    ImGui::EndMenu();
+  }
+}
+
 static void EmitParentContextMenu(NetworkTablesModel* model,
                                   const std::string& path,
                                   NetworkTablesFlags flags) {
-  static char nameBuffer[kTextBufferSize];
   if (ImGui::BeginPopupContextItem(path.c_str())) {
     ImGui::Text("%s", path.c_str());
     ImGui::Separator();
 
-    if (ImGui::BeginMenu("Add new...")) {
-      if (ImGui::IsWindowAppearing()) {
-        nameBuffer[0] = '\0';
-      }
-
-      ImGui::InputTextWithHint("New item name", "example", nameBuffer,
-                               kTextBufferSize);
-      std::string fullNewPath;
-      if (path == "/") {
-        fullNewPath = path + nameBuffer;
-      } else {
-        fullNewPath = fmt::format("{}/{}", path, nameBuffer);
-      }
-
-      ImGui::Text("Adding: %s", fullNewPath.c_str());
-      ImGui::Separator();
-      auto entry = model->GetEntry(fullNewPath);
-      bool exists = entry && entry->info.type != NT_Type::NT_UNASSIGNED;
-      bool enabled = (flags & NetworkTablesFlags_CreateNoncanonicalKeys ||
-                      nameBuffer[0] != '\0') &&
-                     !exists;
-
-      CreateTopicMenuItem(model, fullNewPath, NT_STRING, "string", enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_INTEGER, "int", enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_FLOAT, "float", enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_DOUBLE, "double", enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_BOOLEAN, "boolean", enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_STRING_ARRAY, "string[]",
-                          enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_INTEGER_ARRAY, "int[]",
-                          enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_FLOAT_ARRAY, "float[]",
-                          enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_DOUBLE_ARRAY, "double[]",
-                          enabled);
-      CreateTopicMenuItem(model, fullNewPath, NT_BOOLEAN_ARRAY, "boolean[]",
-                          enabled);
-
-      ImGui::EndMenu();
-    }
+    DisplayNetworkTablesAddMenu(model, path, flags);
 
     ImGui::EndPopup();
   }
@@ -1280,7 +1335,6 @@ static void DisplayTable(NetworkTablesModel* model,
   }
   ImGui::TableHeadersRow();
 
-  // EmitParentContextMenu(model, "/", flags);
   if (flags & NetworkTablesFlags_TreeView) {
     switch (category) {
       case ShowPersistent:
@@ -1511,6 +1565,7 @@ void NetworkTablesView::Display() {
 
 void NetworkTablesView::Settings() {
   m_flags.DisplayMenu();
+  DisplayNetworkTablesAddMenu(m_model, {}, m_flags.GetFlags());
 }
 
 bool NetworkTablesView::HasSettings() {

glass/src/libnt/native/include/glass/networktables/NetworkTables.h

@@ -194,6 +194,10 @@ void DisplayNetworkTables(
     NetworkTablesModel* model,
     NetworkTablesFlags flags = NetworkTablesFlags_Default);
 
+void DisplayNetworkTablesAddMenu(
+    NetworkTablesModel* model, std::string_view path = {},
+    NetworkTablesFlags flags = NetworkTablesFlags_Default);
+
 class NetworkTablesFlagsSettings {
  public:
   explicit NetworkTablesFlagsSettings(

hal/src/main/native/athena/HAL.cpp

@@ -253,12 +253,10 @@ const char* HAL_GetErrorMessage(int32_t code) {
   }
 }
 
+static HAL_RuntimeType runtimeType = HAL_Runtime_RoboRIO;
+
 HAL_RuntimeType HAL_GetRuntimeType(void) {
-  nLoadOut::tTargetClass targetClass = nLoadOut::getTargetClass();
-  if (targetClass == nLoadOut::kTargetClass_RoboRIO2) {
-    return HAL_Runtime_RoboRIO2;
-  }
-  return HAL_Runtime_RoboRIO;
+  return runtimeType;
 }
 
 int32_t HAL_GetFPGAVersion(int32_t* status) {
@@ -523,6 +521,13 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     return false;
   }
 
+  nLoadOut::tTargetClass targetClass = nLoadOut::getTargetClass();
+  if (targetClass == nLoadOut::kTargetClass_RoboRIO2) {
+    runtimeType = HAL_Runtime_RoboRIO2;
+  } else {
+    runtimeType = HAL_Runtime_RoboRIO;
+  }
+
   InterruptManager::Initialize(global->getSystemInterface());
 
   hal::InitializeDriverStation();

hal/src/main/native/sim/REVPH.cpp

@@ -201,7 +201,7 @@ int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {
 
   std::scoped_lock lock{pcm->lock};
   auto& data = SimREVPHData[pcm->module].solenoidOutput;
-  uint8_t ret = 0;
+  int32_t ret = 0;
   for (int i = 0; i < kNumREVPHChannels; i++) {
     ret |= (data[i] << i);
   }

ntcore/src/dev/native/cpp/main.cpp

@@ -7,6 +7,7 @@
 #include <cmath>
 #include <cstdlib>
 #include <numeric>
+#include <random>
 #include <string_view>
 #include <thread>
 
@@ -17,12 +18,18 @@
 #include "ntcore_cpp.h"
 
 void bench();
+void stress();
 
 int main(int argc, char* argv[]) {
   if (argc == 2 && std::string_view{argv[1]} == "bench") {
     bench();
     return EXIT_SUCCESS;
   }
+  if (argc == 2 && std::string_view{argv[1]} == "stress") {
+    stress();
+    return EXIT_SUCCESS;
+  }
+
   auto myValue = nt::GetEntry(nt::GetDefaultInstance(), "MyValue");
 
   nt::SetEntryValue(myValue, nt::Value::MakeString("Hello World"));
@@ -106,3 +113,72 @@ void bench() {
   fmt::print("-- Flush --\n");
   PrintTimes(flushTimes);
 }
+
+static std::random_device r;
+static std::mt19937 gen(r());
+static std::uniform_real_distribution<double> dist;
+
+void stress() {
+  auto server = nt::CreateInstance();
+  nt::StartServer(server, "stress.json", "127.0.0.1", 0, 10000);
+  nt::SubscribeMultiple(server, {{std::string_view{}}});
+
+  using namespace std::chrono_literals;
+
+  for (int count = 0; count < 10; ++count) {
+    std::thread{[] {
+      auto client = nt::CreateInstance();
+      nt::SubscribeMultiple(client, {{std::string_view{}}});
+      for (int i = 0; i < 300; ++i) {
+        // sleep a random amount of time
+        std::this_thread::sleep_for(0.1s * dist(gen));
+
+        // connect
+        nt::StartClient4(client, "client");
+        nt::SetServer(client, "127.0.0.1", 10000);
+
+        // sleep a random amount of time
+        std::this_thread::sleep_for(0.1s * dist(gen));
+
+        // disconnect
+        nt::StopClient(client);
+      }
+      nt::DestroyInstance(client);
+    }}.detach();
+
+    std::thread{[server, count] {
+      for (int n = 0; n < 300; ++n) {
+        // sleep a random amount of time
+        std::this_thread::sleep_for(0.01s * dist(gen));
+
+        // create publishers
+        NT_Publisher pub[30];
+        for (int i = 0; i < 30; ++i) {
+          pub[i] =
+              nt::Publish(nt::GetTopic(server, fmt::format("{}_{}", count, i)),
+                          NT_DOUBLE, "double", {});
+        }
+
+        // publish values
+        for (int i = 0; i < 200; ++i) {
+          // sleep a random amount of time between each value set
+          std::this_thread::sleep_for(0.001s * dist(gen));
+          for (int i = 0; i < 30; ++i) {
+            nt::SetDouble(pub[i], dist(gen));
+          }
+          nt::FlushLocal(server);
+        }
+
+        // sleep a random amount of time
+        std::this_thread::sleep_for(0.1s * dist(gen));
+
+        // remove publishers
+        for (int i = 0; i < 30; ++i) {
+          nt::Unpublish(pub[i]);
+        }
+      }
+    }}.detach();
+  }
+
+  std::this_thread::sleep_for(100s);
+}

ntcore/src/main/native/cpp/NetworkClient.cpp

@@ -56,6 +56,7 @@ class NCImpl {
   void StopDSClient();
 
   virtual void TcpConnected(uv::Tcp& tcp) = 0;
+  virtual void ForceDisconnect(std::string_view reason) = 0;
   virtual void Disconnect(std::string_view reason);
 
   // invariants
@@ -99,6 +100,7 @@ class NCImpl3 : public NCImpl {
 
   void HandleLocal();
   void TcpConnected(uv::Tcp& tcp) final;
+  void ForceDisconnect(std::string_view reason) override;
   void Disconnect(std::string_view reason) override;
 
   std::shared_ptr<net3::UvStreamConnection3> m_wire;
@@ -117,6 +119,7 @@ class NCImpl4 : public NCImpl {
   void HandleLocal();
   void TcpConnected(uv::Tcp& tcp) final;
   void WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp);
+  void ForceDisconnect(std::string_view reason) override;
   void Disconnect(std::string_view reason) override;
 
   std::function<void(int64_t serverTimeOffset, int64_t rtt2, bool valid)>
@@ -155,7 +158,9 @@ void NCImpl::SetServers(
       [this, servers = std::move(serversCopy)](uv::Loop&) mutable {
         m_servers = std::move(servers);
         if (m_dsClientServer.first.empty()) {
-          m_parallelConnect->SetServers(m_servers);
+          if (m_parallelConnect) {
+            m_parallelConnect->SetServers(m_servers);
+          }
         }
       });
 }
@@ -167,14 +172,20 @@ void NCImpl::StartDSClient(unsigned int port) {
     }
     m_dsClientServer.second = port == 0 ? NT_DEFAULT_PORT4 : port;
     m_dsClient = wpi::DsClient::Create(m_loop, m_logger);
-    m_dsClient->setIp.connect([this](std::string_view ip) {
-      m_dsClientServer.first = ip;
-      m_parallelConnect->SetServers({{m_dsClientServer}});
-    });
-    m_dsClient->clearIp.connect([this] {
-      m_dsClientServer.first.clear();
-      m_parallelConnect->SetServers(m_servers);
-    });
+    if (m_dsClient) {
+      m_dsClient->setIp.connect([this](std::string_view ip) {
+        m_dsClientServer.first = ip;
+        if (m_parallelConnect) {
+          m_parallelConnect->SetServers({{m_dsClientServer}});
+        }
+      });
+      m_dsClient->clearIp.connect([this] {
+        m_dsClientServer.first.clear();
+        if (m_parallelConnect) {
+          m_parallelConnect->SetServers(m_servers);
+        }
+      });
+    }
   });
 }
 
@@ -191,15 +202,20 @@ void NCImpl::Disconnect(std::string_view reason) {
   if (m_readLocalTimer) {
     m_readLocalTimer->Stop();
   }
-  m_sendValuesTimer->Stop();
+  if (m_sendValuesTimer) {
+    m_sendValuesTimer->Stop();
+  }
   m_localStorage.ClearNetwork();
   m_localQueue.ClearQueue();
   m_connList.RemoveConnection(m_connHandle);
   m_connHandle = 0;
 
   // start trying to connect again
-  uv::Timer::SingleShot(m_loop, kReconnectRate,
-                        [this] { m_parallelConnect->Disconnected(); });
+  uv::Timer::SingleShot(m_loop, kReconnectRate, [this] {
+    if (m_parallelConnect) {
+      m_parallelConnect->Disconnected();
+    }
+  });
 }
 
 NCImpl3::NCImpl3(int inst, std::string_view id,
@@ -212,25 +228,31 @@ NCImpl3::NCImpl3(int inst, std::string_view id,
         [this](uv::Tcp& tcp) { TcpConnected(tcp); });
 
     m_sendValuesTimer = uv::Timer::Create(loop);
-    m_sendValuesTimer->timeout.connect([this] {
-      if (m_clientImpl) {
-        HandleLocal();
-        m_clientImpl->SendPeriodic(m_loop.Now().count(), false);
-      }
-    });
+    if (m_sendValuesTimer) {
+      m_sendValuesTimer->timeout.connect([this] {
+        if (m_clientImpl) {
+          HandleLocal();
+          m_clientImpl->SendPeriodic(m_loop.Now().count(), false);
+        }
+      });
+    }
 
     // set up flush async
     m_flush = uv::Async<>::Create(m_loop);
-    m_flush->wakeup.connect([this] {
-      if (m_clientImpl) {
-        HandleLocal();
-        m_clientImpl->SendPeriodic(m_loop.Now().count(), true);
-      }
-    });
+    if (m_flush) {
+      m_flush->wakeup.connect([this] {
+        if (m_clientImpl) {
+          HandleLocal();
+          m_clientImpl->SendPeriodic(m_loop.Now().count(), true);
+        }
+      });
+    }
     m_flushAtomic = m_flush.get();
 
     m_flushLocal = uv::Async<>::Create(m_loop);
-    m_flushLocal->wakeup.connect([this] { HandleLocal(); });
+    if (m_flushLocal) {
+      m_flushLocal->wakeup.connect([this] { HandleLocal(); });
+    }
     m_flushLocalAtomic = m_flushLocal.get();
   });
 }
@@ -261,8 +283,10 @@ void NCImpl3::TcpConnected(uv::Tcp& tcp) {
   auto clientImpl = std::make_shared<net3::ClientImpl3>(
       m_loop.Now().count(), m_inst, *wire, m_logger, [this](uint32_t repeatMs) {
         DEBUG4("Setting periodic timer to {}", repeatMs);
-        m_sendValuesTimer->Start(uv::Timer::Time{repeatMs},
-                                 uv::Timer::Time{repeatMs});
+        if (m_sendValuesTimer) {
+          m_sendValuesTimer->Start(uv::Timer::Time{repeatMs},
+                                   uv::Timer::Time{repeatMs});
+        }
       });
   clientImpl->Start(
       m_id, [this, wire,
@@ -276,7 +300,9 @@ void NCImpl3::TcpConnected(uv::Tcp& tcp) {
           return;
         }
 
-        m_parallelConnect->Succeeded(tcp);
+        if (m_parallelConnect) {
+          m_parallelConnect->Succeeded(tcp);
+        }
 
         m_wire = std::move(wire);
         m_clientImpl = std::move(clientImpl);
@@ -305,7 +331,7 @@ void NCImpl3::TcpConnected(uv::Tcp& tcp) {
         tcp.closed.connect([this, &tcp] {
           DEBUG3("NT3 TCP connection closed");
           if (!tcp.IsLoopClosing()) {
-            Disconnect(m_wire->GetDisconnectReason());
+            Disconnect(m_wire ? m_wire->GetDisconnectReason() : "unknown");
           }
         });
 
@@ -323,6 +349,12 @@ void NCImpl3::TcpConnected(uv::Tcp& tcp) {
   tcp.StartRead();
 }
 
+void NCImpl3::ForceDisconnect(std::string_view reason) {
+  if (m_wire) {
+    m_wire->Disconnect(reason);
+  }
+}
+
 void NCImpl3::Disconnect(std::string_view reason) {
   INFO("DISCONNECTED NT3 connection: {}", reason);
   m_clientImpl.reset();
@@ -343,34 +375,42 @@ NCImpl4::NCImpl4(
         [this](uv::Tcp& tcp) { TcpConnected(tcp); });
 
     m_readLocalTimer = uv::Timer::Create(loop);
-    m_readLocalTimer->timeout.connect([this] {
-      if (m_clientImpl) {
-        HandleLocal();
-        m_clientImpl->SendControl(m_loop.Now().count());
-      }
-    });
-    m_readLocalTimer->Start(uv::Timer::Time{100}, uv::Timer::Time{100});
+    if (m_readLocalTimer) {
+      m_readLocalTimer->timeout.connect([this] {
+        if (m_clientImpl) {
+          HandleLocal();
+          m_clientImpl->SendControl(m_loop.Now().count());
+        }
+      });
+      m_readLocalTimer->Start(uv::Timer::Time{100}, uv::Timer::Time{100});
+    }
 
     m_sendValuesTimer = uv::Timer::Create(loop);
-    m_sendValuesTimer->timeout.connect([this] {
-      if (m_clientImpl) {
-        HandleLocal();
-        m_clientImpl->SendValues(m_loop.Now().count(), false);
-      }
-    });
+    if (m_sendValuesTimer) {
+      m_sendValuesTimer->timeout.connect([this] {
+        if (m_clientImpl) {
+          HandleLocal();
+          m_clientImpl->SendValues(m_loop.Now().count(), false);
+        }
+      });
+    }
 
     // set up flush async
     m_flush = uv::Async<>::Create(m_loop);
-    m_flush->wakeup.connect([this] {
-      if (m_clientImpl) {
-        HandleLocal();
-        m_clientImpl->SendValues(m_loop.Now().count(), true);
-      }
-    });
+    if (m_flush) {
+      m_flush->wakeup.connect([this] {
+        if (m_clientImpl) {
+          HandleLocal();
+          m_clientImpl->SendValues(m_loop.Now().count(), true);
+        }
+      });
+    }
     m_flushAtomic = m_flush.get();
 
     m_flushLocal = uv::Async<>::Create(m_loop);
-    m_flushLocal->wakeup.connect([this] { HandleLocal(); });
+    if (m_flushLocal) {
+      m_flushLocal->wakeup.connect([this] { HandleLocal(); });
+    }
     m_flushLocalAtomic = m_flushLocal.get();
   });
 }
@@ -418,7 +458,9 @@ void NCImpl4::TcpConnected(uv::Tcp& tcp) {
 }
 
 void NCImpl4::WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp) {
-  m_parallelConnect->Succeeded(tcp);
+  if (m_parallelConnect) {
+    m_parallelConnect->Succeeded(tcp);
+  }
 
   ConnectionInfo connInfo;
   uv::AddrToName(tcp.GetPeer(), &connInfo.remote_ip, &connInfo.remote_port);
@@ -432,8 +474,10 @@ void NCImpl4::WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp) {
       m_loop.Now().count(), m_inst, *m_wire, m_logger, m_timeSyncUpdated,
       [this](uint32_t repeatMs) {
         DEBUG4("Setting periodic timer to {}", repeatMs);
-        m_sendValuesTimer->Start(uv::Timer::Time{repeatMs},
-                                 uv::Timer::Time{repeatMs});
+        if (m_sendValuesTimer) {
+          m_sendValuesTimer->Start(uv::Timer::Time{repeatMs},
+                                   uv::Timer::Time{repeatMs});
+        }
       });
   m_clientImpl->SetLocal(&m_localStorage);
   m_localStorage.StartNetwork(&m_localQueue);
@@ -456,8 +500,19 @@ void NCImpl4::WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp) {
   });
 }
 
+void NCImpl4::ForceDisconnect(std::string_view reason) {
+  if (m_wire) {
+    m_wire->Disconnect(reason);
+  }
+}
+
 void NCImpl4::Disconnect(std::string_view reason) {
-  INFO("DISCONNECTED NT4 connection: {}", reason);
+  std::string realReason;
+  if (m_wire) {
+    realReason = m_wire->GetDisconnectReason();
+  }
+  INFO("DISCONNECTED NT4 connection: {}",
+       realReason.empty() ? reason : realReason);
   m_clientImpl.reset();
   m_wire.reset();
   NCImpl::Disconnect(reason);
@@ -494,7 +549,7 @@ void NetworkClient::SetServers(
 
 void NetworkClient::Disconnect() {
   m_impl->m_loopRunner.ExecAsync(
-      [this](auto&) { m_impl->Disconnect("requested by application"); });
+      [this](auto&) { m_impl->ForceDisconnect("requested by application"); });
 }
 
 void NetworkClient::StartDSClient(unsigned int port) {
@@ -542,7 +597,7 @@ void NetworkClient3::SetServers(
 
 void NetworkClient3::Disconnect() {
   m_impl->m_loopRunner.ExecAsync(
-      [this](auto&) { m_impl->Disconnect("requested by application"); });
+      [this](auto&) { m_impl->ForceDisconnect("requested by application"); });
 }
 
 void NetworkClient3::StartDSClient(unsigned int port) {

ntcore/src/main/native/cpp/NetworkServer.cpp

@@ -255,8 +255,9 @@ void ServerConnection4::ProcessWsUpgrade() {
     m_info.remote_id = dedupName;
     m_server.AddConnection(this, m_info);
     m_websocket->closed.connect([this](uint16_t, std::string_view reason) {
+      auto realReason = m_wire->GetDisconnectReason();
       INFO("DISCONNECTED NT4 client '{}' (from {}): {}", m_info.remote_id,
-           m_connInfo, reason);
+           m_connInfo, realReason.empty() ? reason : realReason);
       ConnectionClosed();
     });
     m_websocket->text.connect([this](std::string_view data, bool) {
@@ -419,36 +420,46 @@ void NSImpl::Init() {
 
   // set up timers
   m_readLocalTimer = uv::Timer::Create(m_loop);
-  m_readLocalTimer->timeout.connect([this] {
-    HandleLocal();
-    m_serverImpl.SendControl(m_loop.Now().count());
-  });
-  m_readLocalTimer->Start(uv::Timer::Time{100}, uv::Timer::Time{100});
+  if (m_readLocalTimer) {
+    m_readLocalTimer->timeout.connect([this] {
+      HandleLocal();
+      m_serverImpl.SendControl(m_loop.Now().count());
+    });
+    m_readLocalTimer->Start(uv::Timer::Time{100}, uv::Timer::Time{100});
+  }
 
   m_savePersistentTimer = uv::Timer::Create(m_loop);
-  m_savePersistentTimer->timeout.connect([this] {
-    if (m_serverImpl.PersistentChanged()) {
-      uv::QueueWork(
-          m_loop,
-          [this, fn = m_persistentFilename,
-           data = m_serverImpl.DumpPersistent()] { SavePersistent(fn, data); },
-          nullptr);
-    }
-  });
-  m_savePersistentTimer->Start(uv::Timer::Time{1000}, uv::Timer::Time{1000});
+  if (m_savePersistentTimer) {
+    m_savePersistentTimer->timeout.connect([this] {
+      if (m_serverImpl.PersistentChanged()) {
+        uv::QueueWork(
+            m_loop,
+            [this, fn = m_persistentFilename,
+             data = m_serverImpl.DumpPersistent()] {
+              SavePersistent(fn, data);
+            },
+            nullptr);
+      }
+    });
+    m_savePersistentTimer->Start(uv::Timer::Time{1000}, uv::Timer::Time{1000});
+  }
 
   // set up flush async
   m_flush = uv::Async<>::Create(m_loop);
-  m_flush->wakeup.connect([this] {
-    HandleLocal();
-    for (auto&& conn : m_connections) {
-      m_serverImpl.SendValues(conn.conn->GetClientId(), m_loop.Now().count());
-    }
-  });
+  if (m_flush) {
+    m_flush->wakeup.connect([this] {
+      HandleLocal();
+      for (auto&& conn : m_connections) {
+        m_serverImpl.SendValues(conn.conn->GetClientId(), m_loop.Now().count());
+      }
+    });
+  }
   m_flushAtomic = m_flush.get();
 
   m_flushLocal = uv::Async<>::Create(m_loop);
-  m_flushLocal->wakeup.connect([this] { HandleLocal(); });
+  if (m_flushLocal) {
+    m_flushLocal->wakeup.connect([this] { HandleLocal(); });
+  }
   m_flushLocalAtomic = m_flushLocal.get();
 
   INFO("Listening on NT3 port {}, NT4 port {}", m_port3, m_port4);

ntcore/src/main/native/cpp/net/ClientImpl.cpp

@@ -32,7 +32,7 @@ static constexpr uint32_t kMinPeriodMs = 5;
 
 // maximum amount of time the wire can be not ready to send another
 // transmission before we close the connection
-static constexpr uint32_t kWireMaxNotReadyMs = 1000;
+static constexpr uint32_t kWireMaxNotReadyUs = 1000000;
 
 namespace {
 
@@ -313,7 +313,9 @@ void CImpl::SendInitialValues() {
 
 bool CImpl::CheckNetworkReady(uint64_t curTimeMs) {
   if (!m_wire.Ready()) {
-    if (m_lastSendMs != 0 && curTimeMs > (m_lastSendMs + kWireMaxNotReadyMs)) {
+    uint64_t lastFlushTime = m_wire.GetLastFlushTime();
+    uint64_t now = wpi::Now();
+    if (lastFlushTime != 0 && now > (lastFlushTime + kWireMaxNotReadyUs)) {
       m_wire.Disconnect("transmit stalled");
     }
     return false;

ntcore/src/main/native/cpp/net/ServerImpl.cpp

@@ -50,7 +50,7 @@ static constexpr uint32_t kMinPeriodMs = 5;
 
 // maximum amount of time the wire can be not ready to send another
 // transmission before we close the connection
-static constexpr uint32_t kWireMaxNotReadyMs = 1000;
+static constexpr uint32_t kWireMaxNotReadyUs = 1000000;
 
 namespace {
 
@@ -78,12 +78,10 @@ class SImpl;
 
 class ClientData {
  public:
-  ClientData(std::string_view originalName, std::string_view name,
-             std::string_view connInfo, bool local,
+  ClientData(std::string_view name, std::string_view connInfo, bool local,
              ServerImpl::SetPeriodicFunc setPeriodic, SImpl& server, int id,
              wpi::Logger& logger)
-      : m_originalName{originalName},
-        m_name{name},
+      : m_name{name},
         m_connInfo{connInfo},
         m_local{local},
         m_setPeriodic{std::move(setPeriodic)},
@@ -114,12 +112,10 @@ class ClientData {
       std::string_view name, bool special,
       wpi::SmallVectorImpl<SubscriberData*>& buf);
 
-  std::string_view GetOriginalName() const { return m_originalName; }
   std::string_view GetName() const { return m_name; }
   int GetId() const { return m_id; }
 
  protected:
-  std::string m_originalName;
   std::string m_name;
   std::string m_connInfo;
   bool m_local;  // local to machine
@@ -143,12 +139,10 @@ class ClientData {
 
 class ClientData4Base : public ClientData, protected ClientMessageHandler {
  public:
-  ClientData4Base(std::string_view originalName, std::string_view name,
-                  std::string_view connInfo, bool local,
+  ClientData4Base(std::string_view name, std::string_view connInfo, bool local,
                   ServerImpl::SetPeriodicFunc setPeriodic, SImpl& server,
                   int id, wpi::Logger& logger)
-      : ClientData{originalName, name,   connInfo, local,
-                   setPeriodic,  server, id,       logger} {}
+      : ClientData{name, connInfo, local, setPeriodic, server, id, logger} {}
 
  protected:
   // ClientMessageHandler interface
@@ -170,8 +164,7 @@ class ClientData4Base : public ClientData, protected ClientMessageHandler {
 class ClientDataLocal final : public ClientData4Base {
  public:
   ClientDataLocal(SImpl& server, int id, wpi::Logger& logger)
-      : ClientData4Base{"", "", "", true, [](uint32_t) {}, server, id, logger} {
-  }
+      : ClientData4Base{"", "", true, [](uint32_t) {}, server, id, logger} {}
 
   void ProcessIncomingText(std::string_view data) final {}
   void ProcessIncomingBinary(std::span<const uint8_t> data) final {}
@@ -189,12 +182,10 @@ class ClientDataLocal final : public ClientData4Base {
 
 class ClientData4 final : public ClientData4Base {
  public:
-  ClientData4(std::string_view originalName, std::string_view name,
-              std::string_view connInfo, bool local, WireConnection& wire,
-              ServerImpl::SetPeriodicFunc setPeriodic, SImpl& server, int id,
-              wpi::Logger& logger)
-      : ClientData4Base{originalName, name,   connInfo, local,
-                        setPeriodic,  server, id,       logger},
+  ClientData4(std::string_view name, std::string_view connInfo, bool local,
+              WireConnection& wire, ServerImpl::SetPeriodicFunc setPeriodic,
+              SImpl& server, int id, wpi::Logger& logger)
+      : ClientData4Base{name, connInfo, local, setPeriodic, server, id, logger},
         m_wire{wire} {}
 
   void ProcessIncomingText(std::string_view data) final;
@@ -246,7 +237,7 @@ class ClientData3 final : public ClientData, private net3::MessageHandler3 {
               net3::WireConnection3& wire, ServerImpl::Connected3Func connected,
               ServerImpl::SetPeriodicFunc setPeriodic, SImpl& server, int id,
               wpi::Logger& logger)
-      : ClientData{"", "", connInfo, local, setPeriodic, server, id, logger},
+      : ClientData{"", connInfo, local, setPeriodic, server, id, logger},
         m_connected{std::move(connected)},
         m_wire{wire},
         m_decoder{*this} {}
@@ -650,6 +641,11 @@ void ClientData4Base::ClientSubscribe(int64_t subuid,
   }
 
   // see if this immediately subscribes to any topics
+  // for transmit efficiency, we want to batch announcements and values, so
+  // send announcements in first loop and remember what we want to send in
+  // second loop.
+  std::vector<TopicData*> dataToSend;
+  dataToSend.reserve(m_server.m_topics.size());
   for (auto&& topic : m_server.m_topics) {
     bool removed = false;
     if (replace) {
@@ -687,11 +683,15 @@ void ClientData4Base::ClientSubscribe(int64_t subuid,
     // send last value
     if (added && !sub->options.topicsOnly && !wasSubscribedValue &&
         topic->lastValue) {
-      DEBUG4("send last value for {} to client {}", topic->name, m_id);
-      SendValue(topic.get(), topic->lastValue, kSendAll);
+      dataToSend.emplace_back(topic.get());
     }
   }
 
+  for (auto topic : dataToSend) {
+    DEBUG4("send last value for {} to client {}", topic->name, m_id);
+    SendValue(topic, topic->lastValue, kSendAll);
+  }
+
   // update meta data
   UpdateMetaClientSub();
 
@@ -943,7 +943,9 @@ void ClientData4::SendOutgoing(uint64_t curTimeMs) {
   }
 
   if (!m_wire.Ready()) {
-    if (m_lastSendMs != 0 && curTimeMs > (m_lastSendMs + kWireMaxNotReadyMs)) {
+    uint64_t lastFlushTime = m_wire.GetLastFlushTime();
+    uint64_t now = wpi::Now();
+    if (lastFlushTime != 0 && now > (lastFlushTime + kWireMaxNotReadyUs)) {
       m_wire.Disconnect("transmit stalled");
     }
     return;
@@ -1114,7 +1116,9 @@ void ClientData3::SendOutgoing(uint64_t curTimeMs) {
   }
 
   if (!m_wire.Ready()) {
-    if (m_lastSendMs != 0 && curTimeMs > (m_lastSendMs + kWireMaxNotReadyMs)) {
+    uint64_t lastFlushTime = m_wire.GetLastFlushTime();
+    uint64_t now = wpi::Now();
+    if (lastFlushTime != 0 && now > (lastFlushTime + kWireMaxNotReadyUs)) {
       m_wire.Disconnect("transmit stalled");
     }
     return;
@@ -1503,39 +1507,29 @@ SImpl::SImpl(wpi::Logger& logger) : m_logger{logger} {
 std::pair<std::string, int> SImpl::AddClient(
     std::string_view name, std::string_view connInfo, bool local,
     WireConnection& wire, ServerImpl::SetPeriodicFunc setPeriodic) {
-  // strip anything after @ in the name
-  name = wpi::split(name, '@').first;
   if (name.empty()) {
     name = "NT4";
   }
   size_t index = m_clients.size();
-  // find an empty slot and check for duplicates
+  // find an empty slot
   // just do a linear search as number of clients is typically small (<10)
-  int duplicateName = 0;
   for (size_t i = 0, end = index; i < end; ++i) {
-    auto& clientData = m_clients[i];
-    if (clientData && clientData->GetOriginalName() == name) {
-      ++duplicateName;
-    } else if (!clientData && index == end) {
+    if (!m_clients[i]) {
       index = i;
+      break;
     }
   }
   if (index == m_clients.size()) {
     m_clients.emplace_back();
   }
 
-  // if duplicate name, de-duplicate
-  std::string dedupName;
-  if (duplicateName > 0) {
-    dedupName = fmt::format("{}@{}", name, duplicateName);
-  } else {
-    dedupName = name;
-  }
+  // ensure name is unique by suffixing index
+  std::string dedupName = fmt::format("{}@{}", name, index);
 
   auto& clientData = m_clients[index];
-  clientData = std::make_unique<ClientData4>(name, dedupName, connInfo, local,
-                                             wire, std::move(setPeriodic),
-                                             *this, index, m_logger);
+  clientData = std::make_unique<ClientData4>(dedupName, connInfo, local, wire,
+                                             std::move(setPeriodic), *this,
+                                             index, m_logger);
 
   // create client meta topics
   clientData->m_metaPub =
@@ -2289,9 +2283,10 @@ void ServerImpl::SendControl(uint64_t curTimeMs) {
 }
 
 void ServerImpl::SendValues(int clientId, uint64_t curTimeMs) {
-  auto client = m_impl->m_clients[clientId].get();
-  client->SendOutgoing(curTimeMs);
-  client->Flush();
+  if (auto client = m_impl->m_clients[clientId].get()) {
+    client->SendOutgoing(curTimeMs);
+    client->Flush();
+  }
 }
 
 void ServerImpl::HandleLocal(std::span<const ClientMessage> msgs) {

ntcore/src/main/native/cpp/net/WebSocketConnection.cpp

@@ -7,6 +7,7 @@
 #include <span>
 
 #include <wpi/SpanExtras.h>
+#include <wpi/timestamp.h>
 #include <wpinet/WebSocket.h>
 
 using namespace nt;
@@ -25,6 +26,12 @@ WebSocketConnection::~WebSocketConnection() {
   for (auto&& buf : m_buf_pool) {
     buf.Deallocate();
   }
+  for (auto&& buf : m_text_buffers) {
+    buf.Deallocate();
+  }
+  for (auto&& buf : m_binary_buffers) {
+    buf.Deallocate();
+  }
 }
 
 void WebSocketConnection::Flush() {
@@ -61,9 +68,11 @@ void WebSocketConnection::Flush() {
   m_binary_buffers.clear();
   m_text_pos = 0;
   m_binary_pos = 0;
+  m_lastFlushTime = wpi::Now();
 }
 
 void WebSocketConnection::Disconnect(std::string_view reason) {
+  m_reason = reason;
   m_ws.Close(1005, reason);
 }
 

ntcore/src/main/native/cpp/net/WebSocketConnection.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <memory>
+#include <string>
+#include <string_view>
 #include <vector>
 
 #include <wpi/SmallVector.h>
@@ -32,8 +34,12 @@ class WebSocketConnection final
 
   void Flush() final;
 
+  uint64_t GetLastFlushTime() const final { return m_lastFlushTime; }
+
   void Disconnect(std::string_view reason) final;
 
+  std::string_view GetDisconnectReason() const { return m_reason; }
+
  private:
   void StartSendText() final;
   void FinishSendText() final;
@@ -64,6 +70,8 @@ class WebSocketConnection final
   size_t m_binary_pos = 0;
   bool m_in_text = false;
   int m_sendsActive = 0;
+  std::string m_reason;
+  uint64_t m_lastFlushTime = 0;
 };
 
 }  // namespace nt::net

ntcore/src/main/native/cpp/net/WireConnection.h

@@ -30,6 +30,8 @@ class WireConnection {
 
   virtual void Flush() = 0;
 
+  virtual uint64_t GetLastFlushTime() const = 0;  // in microseconds
+
   virtual void Disconnect(std::string_view reason) = 0;
 
  protected:

ntcore/src/main/native/cpp/net3/ClientImpl3.cpp

@@ -13,6 +13,7 @@
 #include <wpi/DenseMap.h>
 #include <wpi/StringMap.h>
 #include <wpi/json.h>
+#include <wpi/timestamp.h>
 
 #include "Handle.h"
 #include "Log.h"
@@ -33,7 +34,7 @@ static constexpr uint32_t kMinPeriodMs = 5;
 
 // maximum amount of time the wire can be not ready to send another
 // transmission before we close the connection
-static constexpr uint32_t kWireMaxNotReadyMs = 1000;
+static constexpr uint32_t kWireMaxNotReadyUs = 1000000;
 
 namespace {
 
@@ -303,7 +304,9 @@ void CImpl::SendValue(Writer& out, Entry* entry, const Value& value) {
 
 bool CImpl::CheckNetworkReady(uint64_t curTimeMs) {
   if (!m_wire.Ready()) {
-    if (m_lastSendMs != 0 && curTimeMs > (m_lastSendMs + kWireMaxNotReadyMs)) {
+    uint64_t lastFlushTime = m_wire.GetLastFlushTime();
+    uint64_t now = wpi::Now();
+    if (lastFlushTime != 0 && now > (lastFlushTime + kWireMaxNotReadyUs)) {
       m_wire.Disconnect("transmit stalled");
     }
     return false;

ntcore/src/main/native/cpp/net3/UvStreamConnection3.cpp

@@ -4,6 +4,7 @@
 
 #include "UvStreamConnection3.h"
 
+#include <wpi/timestamp.h>
 #include <wpinet/uv/Stream.h>
 
 using namespace nt;
@@ -33,6 +34,7 @@ void UvStreamConnection3::Flush() {
   });
   m_buffers.clear();
   m_os.reset();
+  m_lastFlushTime = wpi::Now();
 }
 
 void UvStreamConnection3::Disconnect(std::string_view reason) {

ntcore/src/main/native/cpp/net3/UvStreamConnection3.h

@@ -38,6 +38,8 @@ class UvStreamConnection3 final
 
   void Flush() final;
 
+  uint64_t GetLastFlushTime() const final { return m_lastFlushTime; }
+
   void Disconnect(std::string_view reason) final;
 
   std::string_view GetDisconnectReason() const { return m_reason; }
@@ -54,6 +56,7 @@ class UvStreamConnection3 final
   std::vector<wpi::uv::Buffer> m_buf_pool;
   wpi::raw_uv_ostream m_os;
   std::string m_reason;
+  uint64_t m_lastFlushTime = 0;
   int m_sendsActive = 0;
 };
 

ntcore/src/main/native/cpp/net3/WireConnection3.h

@@ -26,6 +26,8 @@ class WireConnection3 {
 
   virtual void Flush() = 0;
 
+  virtual uint64_t GetLastFlushTime() const = 0;  // in microseconds
+
   virtual void Disconnect(std::string_view reason) = 0;
 
  protected:

ntcore/src/test/native/cpp/net/MockWireConnection.h

@@ -32,6 +32,8 @@ class MockWireConnection : public WireConnection {
 
   MOCK_METHOD(void, Flush, (), (override));
 
+  MOCK_METHOD(uint64_t, GetLastFlushTime, (), (const, override));
+
   MOCK_METHOD(void, Disconnect, (std::string_view reason), (override));
 
  protected:

ntcore/src/test/native/cpp/net/ServerImplTest.cpp

@@ -51,7 +51,7 @@ TEST_F(ServerImplTest, AddClient) {
   MockSetPeriodicFunc setPeriodic;
   auto [name, id] = server.AddClient("test", "connInfo", false, wire,
                                      setPeriodic.AsStdFunction());
-  EXPECT_EQ(name, "test");
+  EXPECT_EQ(name, "test@1");
   EXPECT_NE(id, -1);
 }
 
@@ -67,9 +67,9 @@ TEST_F(ServerImplTest, AddDuplicateClient) {
                                        setPeriodic1.AsStdFunction());
   auto [name2, id2] = server.AddClient("test", "connInfo2", false, wire2,
                                        setPeriodic2.AsStdFunction());
-  EXPECT_EQ(name1, "test");
+  EXPECT_EQ(name1, "test@1");
   EXPECT_NE(id1, -1);
-  EXPECT_EQ(name2, "test@1");
+  EXPECT_EQ(name2, "test@2");
   EXPECT_NE(id1, id2);
   EXPECT_NE(id2, -1);
 }

ntcore/src/test/native/cpp/net3/MockWireConnection3.h

@@ -28,6 +28,8 @@ class MockWireConnection3 : public WireConnection3 {
 
   MOCK_METHOD(void, Flush, (), (override));
 
+  MOCK_METHOD(uint64_t, GetLastFlushTime, (), (const, override));
+
   MOCK_METHOD(void, Disconnect, (std::string_view reason), (override));
 
  protected:

outlineviewer/src/main/native/cpp/main.cpp

@@ -119,6 +119,7 @@ static void DisplayGui() {
   gui::EmitViewMenu();
   if (ImGui::BeginMenu("View")) {
     gFlagsSettings.DisplayMenu();
+    glass::DisplayNetworkTablesAddMenu(gModel.get());
     ImGui::EndMenu();
   }
 

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java

@@ -26,7 +26,11 @@ public abstract class CommandBase implements Sendable, Command {
   }
 
   /**
-   * Adds the specified requirements to the command.
+   * Adds the specified subsystems to the requirements of the command. The scheduler will prevent
+   * two commands that require the same subsystem from being scheduled simultaneously.
+   *
+   * <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
+   * this method should normally be called from the command's constructor.
    *
    * @param requirements the requirements to add
    */

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java

@@ -50,6 +50,14 @@ public interface Subsystem {
     CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
   }
 
+  /**
+   * Removes the default command for the subsystem. This will not cancel the default command if it
+   * is currently running.
+   */
+  default void removeDefaultCommand() {
+    CommandScheduler.getInstance().removeDefaultCommand(this);
+  }
+
   /**
    * Gets the default command for this subsystem. Returns null if no default command is currently
    * associated with the subsystem.

wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp

@@ -21,6 +21,10 @@ void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
                                                     std::move(defaultCommand));
 }
 
+void Subsystem::RemoveDefaultCommand() {
+  CommandScheduler::GetInstance().RemoveDefaultCommand(this);
+}
+
 Command* Subsystem::GetDefaultCommand() const {
   return CommandScheduler::GetInstance().GetDefaultCommand(this);
 }

wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h

@@ -28,6 +28,13 @@ class CommandBase : public Command,
   /**
    * Adds the specified Subsystem requirements to the command.
    *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * Note that the scheduler determines the requirements of a command when it
+   * is scheduled, so this method should normally be called from the command's
+   * constructor.
+   *
    * @param requirements the Subsystem requirements to add
    */
   void AddRequirements(std::initializer_list<Subsystem*> requirements);
@@ -35,6 +42,13 @@ class CommandBase : public Command,
   /**
    * Adds the specified Subsystem requirements to the command.
    *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * Note that the scheduler determines the requirements of a command when it
+   * is scheduled, so this method should normally be called from the command's
+   * constructor.
+   *
    * @param requirements the Subsystem requirements to add
    */
   void AddRequirements(std::span<Subsystem* const> requirements);
@@ -42,6 +56,13 @@ class CommandBase : public Command,
   /**
    * Adds the specified Subsystem requirements to the command.
    *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * Note that the scheduler determines the requirements of a command when it
+   * is scheduled, so this method should normally be called from the command's
+   * constructor.
+   *
    * @param requirements the Subsystem requirements to add
    */
   void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
@@ -49,6 +70,13 @@ class CommandBase : public Command,
   /**
    * Adds the specified Subsystem requirement to the command.
    *
+   * The scheduler will prevent two commands that require the same subsystem
+   * from being scheduled simultaneously.
+   *
+   * Note that the scheduler determines the requirements of a command when it
+   * is scheduled, so this method should normally be called from the command's
+   * constructor.
+   *
    * @param requirement the Subsystem requirement to add
    */
   void AddRequirements(Subsystem* requirement);

wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h

@@ -83,6 +83,12 @@ class Subsystem {
    */
   void SetDefaultCommand(CommandPtr&& defaultCommand);
 
+  /**
+   * Removes the default command for the subsystem.  This will not cancel the
+   * default command if it is currently running.
+   */
+  void RemoveDefaultCommand();
+
   /**
    * Gets the default command for this subsystem.  Returns null if no default
    * command is currently associated with the subsystem.

wpilibc/src/main/native/cpp/DataLogManager.cpp

@@ -26,6 +26,7 @@ namespace {
 
 struct Thread final : public wpi::SafeThread {
   Thread(std::string_view dir, std::string_view filename, double period);
+  ~Thread() override;
 
   void Main() final;
 
@@ -94,6 +95,10 @@ Thread::Thread(std::string_view dir, std::string_view filename, double period)
   StartNTLog();
 }
 
+Thread::~Thread() {
+  StopNTLog();
+}
+
 void Thread::Main() {
   // based on free disk space, scan for "old" FRC_*.wpilog files and remove
   {

wpilibc/src/main/native/cpp/PneumaticHub.cpp

@@ -161,9 +161,14 @@ void PneumaticHub::EnableCompressorAnalog(
                         "maxPressure must be between 0 and 120 PSI, got {}",
                         maxPressure);
   }
-  int32_t status = 0;
+
+  // Send the voltage as it would be if the 5V rail was at exactly 5V.
+  // The firmware will compensate for the real 5V rail voltage, which
+  // can fluctuate somewhat over time.
   units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
   units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+
+  int32_t status = 0;
   HAL_SetREVPHClosedLoopControlAnalog(m_handle, minAnalogVoltage.value(),
                                       maxAnalogVoltage.value(), &status);
   FRC_ReportError(status, "Module {}", m_module);
@@ -186,9 +191,14 @@ void PneumaticHub::EnableCompressorHybrid(
                         "maxPressure must be between 0 and 120 PSI, got {}",
                         maxPressure);
   }
-  int32_t status = 0;
+
+  // Send the voltage as it would be if the 5V rail was at exactly 5V.
+  // The firmware will compensate for the real 5V rail voltage, which
+  // can fluctuate somewhat over time.
   units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
   units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+
+  int32_t status = 0;
   HAL_SetREVPHClosedLoopControlHybrid(m_handle, minAnalogVoltage.value(),
                                       maxAnalogVoltage.value(), &status);
   FRC_ReportError(status, "Module {}", m_module);

wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp

@@ -9,9 +9,11 @@
 
 using namespace frc::sim;
 
-DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder) {
-  frc::sim::SimDeviceSim deviceSim{"DutyCycle:DutyCycleEncoder",
-                                   encoder.GetSourceChannel()};
+DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder)
+    : DutyCycleEncoderSim{encoder.GetSourceChannel()} {}
+
+DutyCycleEncoderSim::DutyCycleEncoderSim(int channel) {
+  frc::sim::SimDeviceSim deviceSim{"DutyCycle:DutyCycleEncoder", channel};
   m_simPosition = deviceSim.GetDouble("position");
   m_simDistancePerRotation = deviceSim.GetDouble("distance_per_rot");
 }

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -15,6 +15,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/timestamp.h>
 #include <wpimath/MathShared.h>
 
 #include "WPILibVersion.h"
@@ -138,6 +139,10 @@ class WPILibMathShared : public wpi::math::MathShared {
         break;
     }
   }
+
+  units::second_t GetTimestamp() override {
+    return units::second_t{wpi::Now() * 1.0e-6};
+  }
 };
 }  // namespace
 

wpilibc/src/main/native/include/frc/PneumaticHub.h

@@ -46,9 +46,10 @@ class PneumaticHub : public PneumaticsBase {
    * and will turn off when the pressure reaches {@code maxPressure}.
    *
    * @param minPressure The minimum pressure. The compressor will turn on when
-   * the pressure drops below this value.
+   * the pressure drops below this value. Range 0 - 120 PSI.
    * @param maxPressure The maximum pressure. The compressor will turn off when
-   * the pressure reaches this value.
+   * the pressure reaches this value. Range 0 - 120 PSI. Must be larger then
+   * minPressure.
    */
   void EnableCompressorAnalog(
       units::pounds_per_square_inch_t minPressure,
@@ -74,10 +75,11 @@ class PneumaticHub : public PneumaticsBase {
    *
    * @param minPressure The minimum pressure. The compressor will turn on when
    * the pressure drops below this value and the pressure switch indicates that
-   * the system is not full.
+   * the system is not full.  Range 0 - 120 PSI.
    * @param maxPressure The maximum pressure. The compressor will turn off when
    * the pressure reaches this value or the pressure switch is disconnected or
-   * indicates that the system is full.
+   * indicates that the system is full. Range 0 - 120 PSI. Must be larger then
+   * minPressure.
    */
   void EnableCompressorHybrid(
       units::pounds_per_square_inch_t minPressure,

wpilibc/src/main/native/include/frc/XboxController.h

@@ -283,9 +283,9 @@ class XboxController : public GenericHID {
   BooleanEvent Y(EventLoop* loop) const;
 
   /**
-   * Whether the Y button was released since the last check.
+   * Read the value of the back button on the controller.
    *
-   * @return Whether the button was released since the last check.
+   * @return The state of the button.
    */
   bool GetBackButton() const;
 

wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h

@@ -26,7 +26,14 @@ class DutyCycleEncoderSim {
   explicit DutyCycleEncoderSim(const DutyCycleEncoder& encoder);
 
   /**
-   * Set the position tin turns.
+   * Constructs from a digital input channel.
+   *
+   * @param channel digital input channel
+   */
+  explicit DutyCycleEncoderSim(int channel);
+
+  /**
+   * Set the position in turns.
    *
    * @param turns The position.
    */

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h

@@ -29,6 +29,10 @@ namespace frc {
 template <class T>
 class SendableChooser : public SendableChooserBase {
   wpi::StringMap<T> m_choices;
+  static_assert(std::is_copy_constructible_v<T>,
+                "T must be copy-constructible!");
+  static_assert(std::is_default_constructible_v<T>,
+                "T must be default-constructible!");
 
   template <class U>
   static U _unwrap_smart_ptr(const U& value);

wpilibc/src/test/native/cpp/NotifierTest.cpp

@@ -0,0 +1,52 @@
+// 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.
+
+#include <atomic>
+
+#include "frc/Notifier.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace {
+
+class NotifierTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    sim::PauseTiming();
+    sim::RestartTiming();
+  }
+
+  void TearDown() override { sim::ResumeTiming(); }
+};
+
+}  // namespace
+
+TEST_F(NotifierTest, StartPeriodicAndStop) {
+  std::atomic<uint32_t> counter{0};
+
+  Notifier notifier{[&] { ++counter; }};
+  notifier.StartPeriodic(1_s);
+
+  sim::StepTiming(10.5_s);
+
+  notifier.Stop();
+  EXPECT_EQ(10u, counter);
+
+  sim::StepTiming(3_s);
+
+  EXPECT_EQ(10u, counter);
+}
+
+TEST_F(NotifierTest, StartSingle) {
+  std::atomic<uint32_t> counter{0};
+
+  Notifier notifier{[&] { ++counter; }};
+  notifier.StartSingle(1_s);
+
+  sim::StepTiming(10.5_s);
+
+  EXPECT_EQ(1u, counter);
+}

wpilibc/src/test/native/cpp/TimedRobotTest.cpp

@@ -16,6 +16,8 @@
 
 using namespace frc;
 
+inline constexpr auto kPeriod = 20_ms;
+
 namespace {
 class TimedRobotTest : public ::testing::TestWithParam<bool> {
  protected:
@@ -45,6 +47,8 @@ class MockRobot : public TimedRobot {
   std::atomic<uint32_t> m_teleopPeriodicCount{0};
   std::atomic<uint32_t> m_testPeriodicCount{0};
 
+  MockRobot() : TimedRobot{kPeriod} {}
+
   void RobotInit() override { m_robotInitCount++; }
 
   void SimulationInit() override { m_simulationInitCount++; }
@@ -107,7 +111,7 @@ TEST_F(TimedRobotTest, DisabledMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -128,7 +132,7 @@ TEST_F(TimedRobotTest, DisabledMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -183,7 +187,7 @@ TEST_F(TimedRobotTest, AutonomousMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -204,7 +208,7 @@ TEST_F(TimedRobotTest, AutonomousMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -259,7 +263,7 @@ TEST_F(TimedRobotTest, TeleopMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -280,7 +284,7 @@ TEST_F(TimedRobotTest, TeleopMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -339,7 +343,7 @@ TEST_P(TimedRobotTest, TestMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -363,7 +367,7 @@ TEST_P(TimedRobotTest, TestMode) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -436,7 +440,7 @@ TEST_F(TimedRobotTest, ModeChange) {
   EXPECT_EQ(0u, robot.m_teleopExitCount);
   EXPECT_EQ(0u, robot.m_testExitCount);
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(0u, robot.m_autonomousInitCount);
@@ -454,7 +458,7 @@ TEST_F(TimedRobotTest, ModeChange) {
   frc::sim::DriverStationSim::SetTest(false);
   frc::sim::DriverStationSim::NotifyNewData();
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_autonomousInitCount);
@@ -472,7 +476,7 @@ TEST_F(TimedRobotTest, ModeChange) {
   frc::sim::DriverStationSim::SetTest(false);
   frc::sim::DriverStationSim::NotifyNewData();
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_autonomousInitCount);
@@ -490,7 +494,7 @@ TEST_F(TimedRobotTest, ModeChange) {
   frc::sim::DriverStationSim::SetTest(true);
   frc::sim::DriverStationSim::NotifyNewData();
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_autonomousInitCount);
@@ -508,7 +512,7 @@ TEST_F(TimedRobotTest, ModeChange) {
   frc::sim::DriverStationSim::SetTest(false);
   frc::sim::DriverStationSim::NotifyNewData();
 
-  frc::sim::StepTiming(20_ms);
+  frc::sim::StepTiming(kPeriod);
 
   EXPECT_EQ(2u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_autonomousInitCount);
@@ -528,7 +532,7 @@ TEST_F(TimedRobotTest, AddPeriodic) {
   MockRobot robot;
 
   std::atomic<uint32_t> callbackCount{0};
-  robot.AddPeriodic([&] { callbackCount++; }, 10_ms);
+  robot.AddPeriodic([&] { callbackCount++; }, kPeriod / 2.0);
 
   std::thread robotThread{[&] { robot.StartCompetition(); }};
 
@@ -540,13 +544,13 @@ TEST_F(TimedRobotTest, AddPeriodic) {
   EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
   EXPECT_EQ(0u, callbackCount);
 
-  frc::sim::StepTiming(10_ms);
+  frc::sim::StepTiming(kPeriod / 2.0);
 
   EXPECT_EQ(0u, robot.m_disabledInitCount);
   EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
   EXPECT_EQ(1u, callbackCount);
 
-  frc::sim::StepTiming(10_ms);
+  frc::sim::StepTiming(kPeriod / 2.0);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
@@ -560,14 +564,14 @@ TEST_F(TimedRobotTest, AddPeriodicWithOffset) {
   MockRobot robot;
 
   std::atomic<uint32_t> callbackCount{0};
-  robot.AddPeriodic([&] { callbackCount++; }, 10_ms, 5_ms);
+  robot.AddPeriodic([&] { callbackCount++; }, kPeriod / 2.0, kPeriod / 4.0);
 
   // Expirations in this test (ms)
   //
   // Robot | Callback
   // ================
-  //    20 |      15
-  //    40 |      25
+  //     p |    0.75p
+  //    2p |    1.25p
 
   std::thread robotThread{[&] { robot.StartCompetition(); }};
 
@@ -579,25 +583,25 @@ TEST_F(TimedRobotTest, AddPeriodicWithOffset) {
   EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
   EXPECT_EQ(0u, callbackCount);
 
-  frc::sim::StepTiming(7.5_ms);
+  frc::sim::StepTiming(kPeriod * 3.0 / 8.0);
 
   EXPECT_EQ(0u, robot.m_disabledInitCount);
   EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
   EXPECT_EQ(0u, callbackCount);
 
-  frc::sim::StepTiming(7.5_ms);
+  frc::sim::StepTiming(kPeriod * 3.0 / 8.0);
 
   EXPECT_EQ(0u, robot.m_disabledInitCount);
   EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
   EXPECT_EQ(1u, callbackCount);
 
-  frc::sim::StepTiming(5_ms);
+  frc::sim::StepTiming(kPeriod / 4.0);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
   EXPECT_EQ(1u, callbackCount);
 
-  frc::sim::StepTiming(5_ms);
+  frc::sim::StepTiming(kPeriod / 4.0);
 
   EXPECT_EQ(1u, robot.m_disabledInitCount);
   EXPECT_EQ(1u, robot.m_disabledPeriodicCount);

wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp

@@ -2,150 +2,30 @@
 // 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 <numbers>
+#include "Robot.h"
 
-#include <frc/Encoder.h>
-#include <frc/Joystick.h>
-#include <frc/Preferences.h>
-#include <frc/RobotController.h>
-#include <frc/TimedRobot.h>
-#include <frc/controller/PIDController.h>
-#include <frc/motorcontrol/PWMSparkMax.h>
-#include <frc/simulation/BatterySim.h>
-#include <frc/simulation/EncoderSim.h>
-#include <frc/simulation/RoboRioSim.h>
-#include <frc/simulation/SingleJointedArmSim.h>
-#include <frc/smartdashboard/Mechanism2d.h>
-#include <frc/smartdashboard/MechanismLigament2d.h>
-#include <frc/smartdashboard/MechanismRoot2d.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-#include <frc/system/plant/LinearSystemId.h>
-#include <frc/util/Color.h>
-#include <frc/util/Color8Bit.h>
-#include <units/angle.h>
-#include <units/moment_of_inertia.h>
+void Robot::SimulationPeriodic() {
+  m_arm.SimulationPeriodic();
+}
 
-/**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control an arm.
- */
-class Robot : public frc::TimedRobot {
-  static constexpr int kMotorPort = 0;
-  static constexpr int kEncoderAChannel = 0;
-  static constexpr int kEncoderBChannel = 1;
-  static constexpr int kJoystickPort = 0;
+void Robot::TeleopInit() {
+  m_arm.LoadPreferences();
+}
 
-  static constexpr std::string_view kArmPositionKey = "ArmPosition";
-  static constexpr std::string_view kArmPKey = "ArmP";
-
-  // The P gain for the PID controller that drives this arm.
-  double kArmKp = 50.0;
-
-  units::degree_t armPosition = 75.0_deg;
-
-  // distance per pulse = (angle per revolution) / (pulses per revolution)
-  //  = (2 * PI rads) / (4096 pulses)
-  static constexpr double kArmEncoderDistPerPulse =
-      2.0 * std::numbers::pi / 4096.0;
-
-  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
-  frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2);
-
-  // Standard classes for controlling our arm
-  frc2::PIDController m_controller{kArmKp, 0, 0};
-  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
-  frc::PWMSparkMax m_motor{kMotorPort};
-  frc::Joystick m_joystick{kJoystickPort};
-
-  // Simulation classes help us simulate what's going on, including gravity.
-  // This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg,
-  // 30in overall arm length, range of motion in [-75, 255] degrees, and noise
-  // with a standard deviation of 1 encoder tick.
-  frc::sim::SingleJointedArmSim m_armSim{
-      m_armGearbox,
-      200.0,
-      frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 8_kg),
-      30_in,
-      -75_deg,
-      255_deg,
-      true,
-      {kArmEncoderDistPerPulse}};
-  frc::sim::EncoderSim m_encoderSim{m_encoder};
-
-  // Create a Mechanism2d display of an Arm
-  frc::Mechanism2d m_mech2d{60, 60};
-  frc::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
-  frc::MechanismLigament2d* m_armTower =
-      m_armBase->Append<frc::MechanismLigament2d>(
-          "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue});
-  frc::MechanismLigament2d* m_arm = m_armBase->Append<frc::MechanismLigament2d>(
-      "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow});
-
- public:
-  void RobotInit() override {
-    m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
-
-    // Put Mechanism 2d to SmartDashboard
-    frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
-
-    // Set the Arm position setpoint and P constant to Preferences if the keys
-    // don't already exist
-    if (!frc::Preferences::ContainsKey(kArmPositionKey)) {
-      frc::Preferences::SetDouble(kArmPositionKey, armPosition.value());
-    }
-    if (!frc::Preferences::ContainsKey(kArmPKey)) {
-      frc::Preferences::SetDouble(kArmPKey, kArmKp);
-    }
+void Robot::TeleopPeriodic() {
+  if (m_joystick.GetTrigger()) {
+    // Here, we run PID control like normal.
+    m_arm.ReachSetpoint();
+  } else {
+    // Otherwise, we disable the motor.
+    m_arm.Stop();
   }
+}
 
-  void SimulationPeriodic() override {
-    // In this method, we update our simulation of what our arm is doing
-    // First, we set our "inputs" (voltages)
-    m_armSim.SetInput(frc::Vectord<1>{m_motor.Get() *
-                                      frc::RobotController::GetInputVoltage()});
-
-    // Next, we update it. The standard loop time is 20ms.
-    m_armSim.Update(20_ms);
-
-    // Finally, we set our simulated encoder's readings and simulated battery
-    // voltage
-    m_encoderSim.SetDistance(m_armSim.GetAngle().value());
-    // SimBattery estimates loaded battery voltages
-    frc::sim::RoboRioSim::SetVInVoltage(
-        frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
-
-    // Update the Mechanism Arm angle based on the simulated arm angle
-    m_arm->SetAngle(m_armSim.GetAngle());
-  }
-
-  void TeleopInit() override {
-    // Read Preferences for Arm setpoint and kP on entering Teleop
-    armPosition = units::degree_t{
-        frc::Preferences::GetDouble(kArmPositionKey, armPosition.value())};
-    if (kArmKp != frc::Preferences::GetDouble(kArmPKey, kArmKp)) {
-      kArmKp = frc::Preferences::GetDouble(kArmPKey, kArmKp);
-      m_controller.SetP(kArmKp);
-    }
-  }
-
-  void TeleopPeriodic() override {
-    if (m_joystick.GetTrigger()) {
-      // Here, we run PID control like normal, with a setpoint read from
-      // preferences in degrees.
-      double pidOutput = m_controller.Calculate(
-          m_encoder.GetDistance(), (units::radian_t{armPosition}.value()));
-      m_motor.SetVoltage(units::volt_t{pidOutput});
-    } else {
-      // Otherwise, we disable the motor.
-      m_motor.Set(0.0);
-    }
-  }
-
-  void DisabledInit() override {
-    // This just makes sure that our simulation code knows that the motor's off.
-    m_motor.Set(0.0);
-  }
-};
+void Robot::DisabledInit() {
+  // This just makes sure that our simulation code knows that the motor's off.
+  m_arm.Stop();
+}
 
 #ifndef RUNNING_FRC_TESTS
 int main() {

wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp

@@ -0,0 +1,64 @@
+// 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.
+
+#include "subsystems/Arm.h"
+
+#include <frc/Preferences.h>
+#include <frc/RobotController.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Arm::Arm() {
+  m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
+
+  // Put Mechanism 2d to SmartDashboard
+  frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
+
+  // Set the Arm position setpoint and P constant to Preferences if the keys
+  // don't already exist
+  frc::Preferences::InitDouble(kArmPositionKey, m_armSetpoint.value());
+  frc::Preferences::InitDouble(kArmPKey, m_armKp);
+}
+
+void Arm::SimulationPeriodic() {
+  // In this method, we update our simulation of what our arm is doing
+  // First, we set our "inputs" (voltages)
+  m_armSim.SetInput(
+      frc::Vectord<1>{m_motor.Get() * frc::RobotController::GetInputVoltage()});
+
+  // Next, we update it. The standard loop time is 20ms.
+  m_armSim.Update(20_ms);
+
+  // Finally, we set our simulated encoder's readings and simulated battery
+  // voltage
+  m_encoderSim.SetDistance(m_armSim.GetAngle().value());
+  // SimBattery estimates loaded battery voltages
+  frc::sim::RoboRioSim::SetVInVoltage(
+      frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
+
+  // Update the Mechanism Arm angle based on the simulated arm angle
+  m_arm->SetAngle(m_armSim.GetAngle());
+}
+
+void Arm::LoadPreferences() {
+  // Read Preferences for Arm setpoint and kP on entering Teleop
+  m_armSetpoint = units::degree_t{
+      frc::Preferences::GetDouble(kArmPositionKey, m_armSetpoint.value())};
+  if (m_armKp != frc::Preferences::GetDouble(kArmPKey, m_armKp)) {
+    m_armKp = frc::Preferences::GetDouble(kArmPKey, m_armKp);
+    m_controller.SetP(m_armKp);
+  }
+}
+
+void Arm::ReachSetpoint() {
+  // Here, we run PID control like normal, with a setpoint read from
+  // preferences in degrees.
+  double pidOutput = m_controller.Calculate(
+      m_encoder.GetDistance(), (units::radian_t{m_armSetpoint}.value()));
+  m_motor.SetVoltage(units::volt_t{pidOutput});
+}
+
+void Arm::Stop() {
+  m_motor.Set(0.0);
+}

wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h

@@ -0,0 +1,46 @@
+// 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 <numbers>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+static constexpr int kMotorPort = 0;
+static constexpr int kEncoderAChannel = 0;
+static constexpr int kEncoderBChannel = 1;
+static constexpr int kJoystickPort = 0;
+
+static constexpr std::string_view kArmPositionKey = "ArmPosition";
+static constexpr std::string_view kArmPKey = "ArmP";
+
+static constexpr double kDefaultArmKp = 50.0;
+static constexpr units::degree_t kDefaultArmSetpoint = 75.0_deg;
+
+static constexpr units::radian_t kMinAngle = -75.0_deg;
+static constexpr units::radian_t kMaxAngle = 255.0_deg;
+
+static constexpr double kArmReduction = 200.0;
+static constexpr units::kilogram_t kArmMass = 8.0_kg;
+static constexpr units::meter_t kArmLength = 30.0_in;
+
+// distance per pulse = (angle per revolution) / (pulses per revolution)
+//  = (2 * PI rads) / (4096 pulses)
+static constexpr double kArmEncoderDistPerPulse =
+    2.0 * std::numbers::pi / 4096.0;

wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h

@@ -0,0 +1,26 @@
+// 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 <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+
+#include "subsystems/Arm.h"
+
+/**
+ * This is a sample program to demonstrate the use of arm simulation.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override {}
+  void SimulationPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void DisabledInit() override;
+
+ private:
+  frc::Joystick m_joystick{kJoystickPort};
+  Arm m_arm;
+};

wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h

@@ -0,0 +1,67 @@
+// 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 <frc/Encoder.h>
+#include <frc/controller/ArmFeedforward.h>
+#include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/simulation/SingleJointedArmSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <units/length.h>
+
+#include "Constants.h"
+
+class Arm {
+ public:
+  Arm();
+  void SimulationPeriodic();
+  void LoadPreferences();
+  void ReachSetpoint();
+  void Stop();
+
+ private:
+  // The P gain for the PID controller that drives this arm.
+  double m_armKp = kDefaultArmKp;
+  units::degree_t m_armSetpoint = kDefaultArmSetpoint;
+
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
+  frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2);
+
+  // Standard classes for controlling our arm
+  frc2::PIDController m_controller{m_armKp, 0, 0};
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+  frc::PWMSparkMax m_motor{kMotorPort};
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  // This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg,
+  // 30in overall arm length, range of motion in [-75, 255] degrees, and noise
+  // with a standard deviation of 1 encoder tick.
+  frc::sim::SingleJointedArmSim m_armSim{
+      m_armGearbox,
+      kArmReduction,
+      frc::sim::SingleJointedArmSim::EstimateMOI(kArmLength, kArmMass),
+      kArmLength,
+      kMinAngle,
+      kMaxAngle,
+      true,
+      {kArmEncoderDistPerPulse}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+  // Create a Mechanism2d display of an Arm
+  frc::Mechanism2d m_mech2d{60, 60};
+  frc::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
+  frc::MechanismLigament2d* m_armTower =
+      m_armBase->Append<frc::MechanismLigament2d>(
+          "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue});
+  frc::MechanismLigament2d* m_arm = m_armBase->Append<frc::MechanismLigament2d>(
+      "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow});
+};

wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp

@@ -2,152 +2,35 @@
 // 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 <numbers>
+#include "Robot.h"
 
-#include <frc/Encoder.h>
-#include <frc/Joystick.h>
-#include <frc/RobotController.h>
-#include <frc/StateSpaceUtil.h>
-#include <frc/TimedRobot.h>
-#include <frc/controller/ElevatorFeedforward.h>
-#include <frc/controller/PIDController.h>
-#include <frc/controller/ProfiledPIDController.h>
-#include <frc/motorcontrol/PWMSparkMax.h>
-#include <frc/simulation/BatterySim.h>
-#include <frc/simulation/ElevatorSim.h>
-#include <frc/simulation/EncoderSim.h>
-#include <frc/simulation/RoboRioSim.h>
-#include <frc/smartdashboard/Mechanism2d.h>
-#include <frc/smartdashboard/MechanismLigament2d.h>
-#include <frc/smartdashboard/MechanismRoot2d.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-#include <frc/system/plant/LinearSystemId.h>
-#include <frc/util/Color.h>
-#include <frc/util/Color8Bit.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <units/moment_of_inertia.h>
+#include "Constants.h"
 
-/**
- * This is a sample program to demonstrate the use of elevator simulation.
- */
-class Robot : public frc::TimedRobot {
-  static constexpr int kMotorPort = 0;
-  static constexpr int kEncoderAChannel = 0;
-  static constexpr int kEncoderBChannel = 1;
-  static constexpr int kJoystickPort = 0;
+void Robot::RobotPeriodic() {
+  // Update the telemetry, including mechanism visualization, regardless of
+  // mode.
+  m_elevator.UpdateTelemetry();
+}
 
-  static constexpr double kElevatorKp = 5.0;
-  static constexpr double kElevatorKi = 0.0;
-  static constexpr double kElevatorKd = 0.0;
+void Robot::SimulationPeriodic() {
+  // Update the simulation model.
+  m_elevator.SimulationPeriodic();
+}
 
-  static constexpr units::volt_t kElevatorkS = 0.0_V;
-  static constexpr units::volt_t kElevatorkG = 0.0_V;
-  static constexpr auto kElevatorkV = 0.762_V / 1_mps;
-  static constexpr auto kElevatorkA = 0.762_V / 1_mps_sq;
-
-  static constexpr double kElevatorGearing = 10.0;
-  static constexpr units::meter_t kElevatorDrumRadius = 2_in;
-  static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
-
-  static constexpr units::meter_t kSetpoint = 30_in;
-  static constexpr units::meter_t kMinElevatorHeight = 2_in;
-  static constexpr units::meter_t kMaxElevatorHeight = 50_in;
-
-  // distance per pulse = (distance per revolution) / (pulses per revolution)
-  //  = (Pi * D) / ppr
-  static constexpr double kArmEncoderDistPerPulse =
-      2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
-
-  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
-  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
-
-  // Standard classes for controlling our elevator
-  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{2.45_mps,
-                                                                  2.45_mps_sq};
-  frc::ProfiledPIDController<units::meters> m_controller{
-      kElevatorKp, kElevatorKi, kElevatorKd, m_constraints};
-
-  frc::ElevatorFeedforward m_feedforward{kElevatorkS, kElevatorkG, kElevatorkV,
-                                         kElevatorkA};
-  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
-  frc::PWMSparkMax m_motor{kMotorPort};
-  frc::Joystick m_joystick{kJoystickPort};
-
-  // Simulation classes help us simulate what's going on, including gravity.
-  frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
-                                      kElevatorGearing,
-                                      kCarriageMass,
-                                      kElevatorDrumRadius,
-                                      kMinElevatorHeight,
-                                      kMaxElevatorHeight,
-                                      true,
-                                      {0.01}};
-  frc::sim::EncoderSim m_encoderSim{m_encoder};
-
-  // Create a Mechanism2d display of an elevator
-  frc::Mechanism2d m_mech2d{20, 50};
-  frc::MechanismRoot2d* m_elevatorRoot =
-      m_mech2d.GetRoot("Elevator Root", 10, 0);
-  frc::MechanismLigament2d* m_elevatorMech2d =
-      m_elevatorRoot->Append<frc::MechanismLigament2d>(
-          "Elevator", units::inch_t{m_elevatorSim.GetPosition()}.value(),
-          90_deg);
-
- public:
-  void RobotInit() override {
-    m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
-
-    // Put Mechanism 2d to SmartDashboard
-    // To view the Elevator Sim in the simulator, select Network Tables ->
-    // SmartDashboard -> Elevator Sim
-    frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
+void Robot::TeleopPeriodic() {
+  if (m_joystick.GetTrigger()) {
+    // Here, we set the constant setpoint of 0.75 meters.
+    m_elevator.ReachGoal(Constants::kSetpoint);
+  } else {
+    // Otherwise, we update the setpoint to 0.
+    m_elevator.ReachGoal(0.0_m);
   }
+}
 
-  void SimulationPeriodic() override {
-    // In this method, we update our simulation of what our elevator is doing
-    // First, we set our "inputs" (voltages)
-    m_elevatorSim.SetInput(frc::Vectord<1>{
-        m_motor.Get() * frc::RobotController::GetInputVoltage()});
-
-    // Next, we update it. The standard loop time is 20ms.
-    m_elevatorSim.Update(20_ms);
-
-    // Finally, we set our simulated encoder's readings and simulated battery
-    // voltage
-    m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
-    // SimBattery estimates loaded battery voltages
-    frc::sim::RoboRioSim::SetVInVoltage(
-        frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
-
-    // Update the Elevator length based on the simulated elevator height
-    m_elevatorMech2d->SetLength(
-        units::inch_t{m_elevatorSim.GetPosition()}.value());
-  }
-
-  void TeleopPeriodic() override {
-    if (m_joystick.GetTrigger()) {
-      // Here, we set the constant setpoint of 30in.
-      m_controller.SetGoal(kSetpoint);
-    } else {
-      // Otherwise, we update the setpoint to 0.
-      m_controller.SetGoal(0.0_m);
-    }
-    // With the setpoint value we run PID control like normal
-    double pidOutput =
-        m_controller.Calculate(units::meter_t{m_encoder.GetDistance()});
-    units::volt_t feedforwardOutput =
-        m_feedforward.Calculate(m_controller.GetSetpoint().velocity);
-    m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
-  }
-  // To view the Elevator in the simulator, select Network Tables ->
-  // SmartDashboard -> Elevator Sim
-
-  void DisabledInit() override {
-    // This just makes sure that our simulation code knows that the motor's off.
-    m_motor.Set(0.0);
-  }
-};
+void Robot::DisabledInit() {
+  // This just makes sure that our simulation code knows that the motor's off.
+  m_elevator.Stop();
+}
 
 #ifndef RUNNING_FRC_TESTS
 int main() {

wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp

@@ -0,0 +1,55 @@
+// 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.
+
+#include "subsystems/Elevator.h"
+
+#include <frc/RobotController.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Elevator::Elevator() {
+  m_encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse);
+
+  // Put Mechanism 2d to SmartDashboard
+  // To view the Elevator visualization, select Network Tables -> SmartDashboard
+  // -> Elevator Sim
+  frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
+}
+
+void Elevator::SimulationPeriodic() {
+  // In this method, we update our simulation of what our elevator is doing
+  // First, we set our "inputs" (voltages)
+  m_elevatorSim.SetInput(frc::Vectord<1>{
+      m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()});
+
+  // Next, we update it. The standard loop time is 20ms.
+  m_elevatorSim.Update(20_ms);
+
+  // Finally, we set our simulated encoder's readings and simulated battery
+  // voltage
+  m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
+  // SimBattery estimates loaded battery voltages
+  frc::sim::RoboRioSim::SetVInVoltage(
+      frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
+}
+
+void Elevator::UpdateTelemetry() {
+  // Update the Elevator length based on the simulated elevator height
+  m_elevatorMech2d->SetLength(m_encoder.GetDistance());
+}
+
+void Elevator::ReachGoal(units::meter_t goal) {
+  m_controller.SetGoal(goal);
+  // With the setpoint value we run PID control like normal
+  double pidOutput =
+      m_controller.Calculate(units::meter_t{m_encoder.GetDistance()});
+  units::volt_t feedforwardOutput =
+      m_feedforward.Calculate(m_controller.GetSetpoint().velocity);
+  m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
+}
+
+void Elevator::Stop() {
+  m_controller.SetGoal(0.0_m);
+  m_motor.Set(0.0);
+}

wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h

@@ -0,0 +1,55 @@
+// 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 <numbers>
+
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace Constants {
+
+static constexpr int kMotorPort = 0;
+static constexpr int kEncoderAChannel = 0;
+static constexpr int kEncoderBChannel = 1;
+static constexpr int kJoystickPort = 0;
+
+static constexpr double kElevatorKp = 5.0;
+static constexpr double kElevatorKi = 0.0;
+static constexpr double kElevatorKd = 0.0;
+
+static constexpr units::volt_t kElevatorkS = 0.0_V;
+static constexpr units::volt_t kElevatorkG = 0.762_V;
+static constexpr auto kElevatorkV = 0.762_V / 1_mps;
+static constexpr auto kElevatorkA = 0.0_V / 1_mps_sq;
+
+static constexpr double kElevatorGearing = 10.0;
+static constexpr units::meter_t kElevatorDrumRadius = 2_in;
+static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+
+static constexpr units::meter_t kSetpoint = 75_cm;
+static constexpr units::meter_t kMinElevatorHeight = 0_cm;
+static constexpr units::meter_t kMaxElevatorHeight = 1.25_m;
+
+// distance per pulse = (distance per revolution) / (pulses per revolution)
+//  = (Pi * D) / ppr
+static constexpr double kArmEncoderDistPerPulse =
+    2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
+
+}  // namespace Constants

wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Robot.h

@@ -0,0 +1,26 @@
+// 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 <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+
+#include "subsystems/Elevator.h"
+
+/**
+ * This is a sample program to demonstrate the use of elevator simulation.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override {}
+  void RobotPeriodic() override;
+  void SimulationPeriodic() override;
+  void TeleopPeriodic() override;
+  void DisabledInit() override;
+
+ private:
+  frc::Joystick m_joystick{Constants::kJoystickPort};
+  Elevator m_elevator;
+};

wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h

@@ -0,0 +1,69 @@
+// 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 <frc/Encoder.h>
+#include <frc/controller/ElevatorFeedforward.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/ElevatorSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <units/length.h>
+
+#include "Constants.h"
+
+class Elevator {
+ public:
+  Elevator();
+  void SimulationPeriodic();
+  void UpdateTelemetry();
+  void ReachGoal(units::meter_t goal);
+  void Stop();
+
+ private:
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{2.45_mps,
+                                                                  2.45_mps_sq};
+  frc::ProfiledPIDController<units::meters> m_controller{
+      Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd,
+      m_constraints};
+
+  frc::ElevatorFeedforward m_feedforward{
+      Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV,
+      Constants::kElevatorkA};
+  frc::Encoder m_encoder{Constants::kEncoderAChannel,
+                         Constants::kEncoderBChannel};
+  frc::PWMSparkMax m_motor{Constants::kMotorPort};
+  frc::sim::PWMSim m_motorSim{m_motor};
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
+                                      Constants::kElevatorGearing,
+                                      Constants::kCarriageMass,
+                                      Constants::kElevatorDrumRadius,
+                                      Constants::kMinElevatorHeight,
+                                      Constants::kMaxElevatorHeight,
+                                      true,
+                                      {0.01}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+  // Create a Mechanism2d display of an elevator
+  frc::Mechanism2d m_mech2d{20, 50};
+  frc::MechanismRoot2d* m_elevatorRoot =
+      m_mech2d.GetRoot("Elevator Root", 10, 0);
+  frc::MechanismLigament2d* m_elevatorMech2d =
+      m_elevatorRoot->Append<frc::MechanismLigament2d>(
+          "Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
+};

wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp

@@ -21,7 +21,7 @@ class Robot : public frc::TimedRobot {
   }
 
   void TeleopPeriodic() override {
-    /* Use the joystick X axis for forward movement, Y axis for lateral
+    /* Use the joystick Y axis for forward movement, X axis for lateral
      * movement, and Z axis for rotation.
      */
     m_robotDrive.DriveCartesian(-m_stick.GetY(), -m_stick.GetX(),

wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h

@@ -25,7 +25,7 @@ class Robot : public frc::TimedRobot {
 
   static constexpr int kPotChannel = 1;
   static constexpr int kMotorChannel = 7;
-  static constexpr int kJoystickChannel = 0;
+  static constexpr int kJoystickChannel = 3;
 
   // The elevator can move 1.5 meters from top to bottom
   static constexpr units::meter_t kFullHeight = 1.5_m;

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -1,7 +1,7 @@
 [
   {
     "name": "Motor Control",
-    "description": "Demonstrate controlling a single motor with a Joystick and displaying the net movement of the motor using an encoder.",
+    "description": "Control a single motor with a joystick, displaying the movement of the motor using an encoder.",
     "tags": [
       "Basic Robot",
       "Encoder",
@@ -14,7 +14,7 @@
   },
   {
     "name": "Relay",
-    "description": "Demonstrate controlling a Relay from Joystick buttons.",
+    "description": "Control a relay from joystick buttons.",
     "tags": [
       "Hardware",
       "Relay",
@@ -26,7 +26,7 @@
   },
   {
     "name": "PDP CAN Monitoring",
-    "description": "Demonstrate using CAN to monitor the voltage, current, and temperature in the Power Distribution Panel.",
+    "description": "Monitor Power Distribution data such as voltage, current, temperature, etc.",
     "tags": [
       "Hardware",
       "PDP",
@@ -40,7 +40,7 @@
     "name": "Mechanism2d",
     "foldername": "Mechanism2d",
     "gradlebase": "cpp",
-    "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "description": "Display mechanism states on a dashboard with Mechanism2d.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -54,7 +54,7 @@
   },
   {
     "name": "Solenoids",
-    "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.",
+    "description": "Control a single and double solenoid from joystick buttons.",
     "tags": [
       "Hardware",
       "Joystick",
@@ -66,7 +66,7 @@
   },
   {
     "name": "Encoder",
-    "description": "Demonstrate displaying the value of a quadrature encoder on the SmartDashboard.",
+    "description": "View values from a quadrature encoder.",
     "tags": [
       "Hardware",
       "Encoder",
@@ -78,7 +78,7 @@
   },
   {
     "name": "EventLoop",
-    "description": "Demonstrate managing a ball system using EventLoop and BooleanEvent.",
+    "description": "Manage a ball system using EventLoop and BooleanEvent.",
     "tags": [
       "Basic Robot",
       "Flywheel",
@@ -90,7 +90,7 @@
   },
   {
     "name": "Arcade Drive",
-    "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class",
+    "description": "Control a differential drivetrain with single-joystick arcade drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -102,7 +102,7 @@
   },
   {
     "name": "Tank Drive",
-    "description": "An example program which demonstrates the use of Tank Drive with the DifferentialDrive class",
+    "description": "Control a differential drive with twin-joystick tank drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -114,7 +114,7 @@
   },
   {
     "name": "Mecanum Drive",
-    "description": "An example program which demonstrates the use of Mecanum Drive with the MecanumDrive class",
+    "description": "Control a mecanum drivetrain with a joystick in teleop.",
     "tags": [
       "Basic Robot",
       "Mecanum Drive",
@@ -126,7 +126,7 @@
   },
   {
     "name": "Ultrasonic",
-    "description": "Demonstrate using the Ultrasonic class with a ping-response ultrasonic sensor.",
+    "description": "View values from a ping-response ultrasonic sensor.",
     "tags": [
       "Hardware",
       "Ultrasonic",
@@ -139,7 +139,7 @@
   },
   {
     "name": "UltrasonicPID",
-    "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.",
+    "description": "Maintain a set distance from an obstacle with an ultrasonic sensor and PID control.",
     "tags": [
       "Basic Robot",
       "Ultrasonic",
@@ -152,7 +152,7 @@
   },
   {
     "name": "Gyro",
-    "description": "An example program showing how to drive straight with using a gyro sensor.",
+    "description": "Drive a differential drive straight with a gyro sensor.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -167,7 +167,7 @@
   },
   {
     "name": "Gyro Mecanum",
-    "description": "An example program showing how to perform mecanum drive with field oriented controls.",
+    "description": "Drive a mecanum drivetrain using field-oriented controls with a joystick.",
     "tags": [
       "Basic Robot",
       "Mecanum Drive",
@@ -181,7 +181,7 @@
   },
   {
     "name": "HID Rumble",
-    "description": "An example program showing how to make human interface devices rumble.",
+    "description": "Make human interface devices (HID) rumble.",
     "tags": [
       "Hardware",
       "XboxController"
@@ -192,7 +192,7 @@
   },
   {
     "name": "PotentiometerPID",
-    "description": "An example to demonstrate the use of a potentiometer and PID control to maintain elevator position setpoints.",
+    "description": "Maintain elevator position setpoints with a potentiometer and PID control.",
     "tags": [
       "Basic Robot",
       "Analog",
@@ -206,7 +206,7 @@
   },
   {
     "name": "Elevator with trapezoid profiled PID",
-    "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+    "description": "Reach elevator position setpoints with trapezoid profiles and smart motor controller PID.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -220,7 +220,7 @@
   },
   {
     "name": "Elevator with profiled PID controller",
-    "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+    "description": "Reach elevator position setpoints with an encoder and profiled PID control.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -233,7 +233,7 @@
   },
   {
     "name": "Getting Started",
-    "description": "An example program which demonstrates the simplest autonomous and teleoperated routines.",
+    "description": "A differential-drive robot with split-stick Xbox arcade drive with a simple time-based autonomous.",
     "tags": [
       "Basic Robot"
     ],
@@ -243,7 +243,7 @@
   },
   {
     "name": "Simple Vision",
-    "description": "The minimal program to acquire images from an attached USB camera on the robot and send them to the dashboard.",
+    "description": "Use the CameraServer class to stream from a USB Webcam without processing the images.",
     "tags": [
       "Vision"
     ],
@@ -253,7 +253,7 @@
   },
   {
     "name": "Intermediate Vision",
-    "description": "An example program that acquires images from an attached USB camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display.",
+    "description": "Acquire images from an attached USB camera and add some annotation to the image (as you might do for showing operators the result of some image recognition) and send it to the dashboard for display.",
     "tags": [
       "Vision"
     ],
@@ -263,7 +263,7 @@
   },
   {
     "name": "AprilTags Vision",
-    "description": "on-roboRIO detection of AprilTags using an attached USB camera.",
+    "description": "On-roboRIO detection of AprilTags using an attached USB camera.",
     "tags": [
       "Vision",
       "AprilTags"
@@ -274,7 +274,7 @@
   },
   {
     "name": "I2C Communication",
-    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's I2C port",
+    "description": "Communicate with external devices (such as an Arduino) using the roboRIO's I2C port.",
     "tags": [
       "Hardware",
       "I2C"
@@ -285,7 +285,7 @@
   },
   {
     "name": "Digital Communication Sample",
-    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's DIO",
+    "description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",
     "tags": [
       "Hardware",
       "Digital Output"
@@ -296,7 +296,7 @@
   },
   {
     "name": "Axis Camera Sample",
-    "description": "An example program that acquires images from an Axis network camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display. This demonstrates the use of the AxisCamera class.",
+    "description": "Acquire images from an Axis network camera and adds some annotation to the image (as you might do for showing operators the result of some image recognition), and sends it to the dashboard for display.",
     "tags": [
       "Vision"
     ],
@@ -306,7 +306,7 @@
   },
   {
     "name": "GearsBot",
-    "description": "A fully functional example CommandBased program for WPIs GearsBot robot, using the new command-based framework. This code can run on your computer if it supports simulation.",
+    "description": "A fully functional Command-Based program for WPI's GearsBot robot.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -324,7 +324,7 @@
   },
   {
     "name": "HAL",
-    "description": "A program created using the HAL exclusively. This example is for advanced users",
+    "description": "Use the low-level HAL C functions. This example is for advanced users.",
     "tags": [
       "Basic Robot",
       "HAL"
@@ -334,8 +334,8 @@
     "commandversion": 2
   },
   {
-    "name": "ShuffleBoard",
-    "description": "An example program that uses ShuffleBoard with its Widgets and Tabs.",
+    "name": "Shuffleboard",
+    "description": "Present various data via the Shuffleboard API.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -350,7 +350,7 @@
   },
   {
     "name": "'Traditional' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game, written in the 'traditional' style, i.e. commands are given their own classes.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -368,7 +368,7 @@
   },
   {
     "name": "'Inlined' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -386,7 +386,7 @@
   },
   {
     "name": "Rapid React Command Bot",
-    "description": "A fully-functional command-based fender bot for the 2022 game using the new command framework.",
+    "description": "A fully-functional command-based fender bot for the 2022 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -405,7 +405,7 @@
   },
   {
     "name": "Select Command Example",
-    "description": "An example showing how to use the SelectCommand class from the new command framework.",
+    "description": "Use SelectCommand to select an autonomous routine.",
     "tags": [
       "Command-based"
     ],
@@ -415,7 +415,7 @@
   },
   {
     "name": "Frisbeebot",
-    "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework",
+    "description": "A simple frisbee shooter for the 2013 game, demonstrating use of PIDSubsystem.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -431,7 +431,7 @@
   },
   {
     "name": "Gyro Drive Commands",
-    "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.",
+    "description": "Control a robot's angle with PID and a gyro, in command-based.",
     "tags": [
       "Command-based",
       "Differential Drive",
@@ -447,7 +447,7 @@
   },
   {
     "name": "SwerveBot",
-    "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.",
+    "description": "Use kinematics and odometry with a swerve drive.",
     "tags": [
       "Swerve Drive",
       "Odometry",
@@ -461,7 +461,7 @@
   },
   {
     "name": "MecanumBot",
-    "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.",
+    "description": "Use kinematics and odometry with a mecanum drive.",
     "tags": [
       "Mecanum Drive",
       "Odometry",
@@ -475,7 +475,7 @@
   },
   {
     "name": "DifferentialDriveBot",
-    "description": "An example program for a differential drive that uses differential drive kinematics and odometry.",
+    "description": "Use kinematics and odometry with a differential drive.",
     "tags": [
       "Differential Drive",
       "Odometry",
@@ -489,7 +489,7 @@
   },
   {
     "name": "RamseteCommand",
-    "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
+    "description": "Follow a pre-generated trajectory with a differential drive using RamseteCommand.",
     "tags": [
       "Differential Drive",
       "Command-based",
@@ -507,7 +507,7 @@
   },
   {
     "name": "Arcade Drive Xbox Controller",
-    "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class and an Xbox Controller.",
+    "description": "Control a differential drive with split-stick arcade drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -519,7 +519,7 @@
   },
   {
     "name": "Tank Drive Xbox Controller",
-    "description": "An example program which demonstrates the use of Tank Drive with the DifferentialDrive class and an Xbox Controller.",
+    "description": "Control a differential drive with Xbox tank drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -531,7 +531,7 @@
   },
   {
     "name": "Duty Cycle Encoder",
-    "description": "Demonstrates the use of the Duty Cycle Encoder class",
+    "description": "View values from a duty-cycle encoder.",
     "tags": [
       "Hardware",
       "Duty Cycle",
@@ -544,7 +544,7 @@
   },
   {
     "name": "Duty Cycle Input",
-    "description": "Demonstrates the use of the Duty Cycle class",
+    "description": "View duty-cycle input.",
     "tags": [
       "Hardware",
       "Duty Cycle",
@@ -556,7 +556,7 @@
   },
   {
     "name": "Addressable LED",
-    "description": "Demonstrates the use of the Addressable LED class",
+    "description": "Display a rainbow pattern on an addressable LED strip.",
     "tags": [
       "Hardware",
       "Basic Robot",
@@ -568,7 +568,7 @@
   },
   {
     "name": "DMA",
-    "description": "Demonstrates the use of the DMA class",
+    "description": "Read various sensors using DMA.",
     "tags": [
       "Hardware",
       "DMA",
@@ -580,7 +580,7 @@
   },
   {
     "name": "MecanumControllerCommand",
-    "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+    "description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
     "tags": [
       "Command-based",
       "Mecanum Drive",
@@ -597,7 +597,7 @@
   },
   {
     "name": "SwerveControllerCommand",
-    "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+    "description": "Follow a pre-generated trajectory with a swerve drive using SwerveControllerCommand.",
     "tags": [
       "Command-based",
       "Swerve Drive",
@@ -614,7 +614,7 @@
   },
   {
     "name": "ArmBot",
-    "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+    "description": "Control an arm with ProfiledPIDSubsystem.",
     "tags": [
       "Command-based",
       "Arm",
@@ -629,7 +629,7 @@
   },
   {
     "name": "ArmBotOffboard",
-    "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+    "description": "Control an arm with TrapezoidProfileSubsystem and smart motor controller PID.",
     "tags": [
       "Command-based",
       "Arm",
@@ -644,7 +644,7 @@
   },
   {
     "name": "DriveDistanceOffboard",
-    "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+    "description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.",
     "tags": [
       "Command-based",
       "Differential Drive",
@@ -658,7 +658,7 @@
   },
   {
     "name": "RamseteController",
-    "description": "An example robot demonstrating the use of RamseteController.",
+    "description": "Follow a pre-generated trajectory with a differential drive using RamseteController.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -689,7 +689,7 @@
   },
   {
     "name": "StateSpaceFlywheel",
-    "description": "An example state-space controller for a flywheel.",
+    "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Flywheel",
@@ -704,7 +704,7 @@
   },
   {
     "name": "StateSpaceFlywheelSysId",
-    "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.",
+    "description": "Control a flywheel using a state-space model (based on values from SysId), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Flywheel",
@@ -720,7 +720,7 @@
   },
   {
     "name": "StateSpaceElevator",
-    "description": "An example state-space controller for controlling an elevator.",
+    "description": "Control an elevator using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -735,7 +735,7 @@
   },
   {
     "name": "StateSpaceArm",
-    "description": "An example state-space controller for controlling an arm.",
+    "description": "Control an arm using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Arm",
@@ -750,7 +750,7 @@
   },
   {
     "name": "ElevatorSimulation",
-    "description": "Demonstrates the use of physics simulation with a simple elevator.",
+    "description": "Simulate an elevator.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -765,7 +765,7 @@
   },
   {
     "name": "DifferentialDrivePoseEstimator",
-    "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+    "description": "Combine differential-drive odometry with vision data using DifferentialDrivePoseEstimator.",
     "tags": [
       "Differential Drive",
       "State-Space",
@@ -780,7 +780,7 @@
   },
   {
     "name": "MecanumDrivePoseEstimator",
-    "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum odometry.",
+    "description": "Combine mecanum-drive odometry with vision data using MecanumDrivePoseEstimator.",
     "tags": [
       "Mecanum Drive",
       "State-Space",
@@ -795,7 +795,7 @@
   },
   {
     "name": "ArmSimulation",
-    "description": "Demonstrates the use of physics simulation with a simple single-jointed arm.",
+    "description": "Simulate a single-jointed arm.",
     "tags": [
       "Basic Robot",
       "Arm",
@@ -810,7 +810,7 @@
   },
   {
     "name": "UnitTesting",
-    "description": "Demonstrates basic unit testing for a robot project.",
+    "description": "Test a robot project with basic unit tests in simulation.",
     "tags": [
       "Intake",
       "Pneumatics"
@@ -821,7 +821,7 @@
   },
   {
     "name": "SimpleDifferentialDriveSimulation",
-    "description": "An example of a minimal drivetrain simulation project without the command-based library.",
+    "description": "Simulate a differential drivetrain and follow trajectories with RamseteController (non-command-based).",
     "tags": [
       "Differential Drive",
       "State-Space",
@@ -838,7 +838,7 @@
   },
   {
     "name": "StateSpaceDriveSimulation",
-    "description": "Demonstrates the use of physics simulation with a differential drivetrain and the Field2d class.",
+    "description": "Simulate a differential drivetrain and follow trajectories with RamseteCommand (command-based).",
     "tags": [
       "Command-based",
       "Differential Drive",
@@ -852,7 +852,7 @@
   },
   {
     "name": "SwerveDrivePoseEstimator",
-    "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for mecanum drive odometry.",
+    "description": "Combine swerve-drive odometry with vision data using SwerveDrivePoseEstimator.",
     "tags": [
       "Swerve Drive",
       "State-Space",

wpilibcExamples/src/main/cpp/templates/templates.json

@@ -1,7 +1,7 @@
 [
   {
     "name": "Command Robot",
-    "description": "Command style",
+    "description": "Command-based, with explanatory comments and example code.",
     "tags": [
       "Command"
     ],
@@ -11,7 +11,7 @@
   },
   {
     "name": "Command Robot Skeleton (Advanced)",
-    "description": "Skeleton (stub) code for Command Robot",
+    "description": "Skeleton (stub) code for Command-based, without explanatory comments and example code.",
     "tags": [
       "Command",
       "Skeleton"
@@ -22,7 +22,7 @@
   },
   {
     "name": "Timed Robot",
-    "description": "Timed style",
+    "description": "Timed style, with explanatory comments and example code.",
     "tags": [
       "Timed"
     ],
@@ -32,7 +32,7 @@
   },
   {
     "name": "Timed Skeleton (Advanced)",
-    "description": "Skeleton (stub) code for TimedRobot",
+    "description": "Skeleton (stub) code for TimedRobot, without explanatory comments and example code.",
     "tags": [
       "Timed",
       "Skeleton"
@@ -43,7 +43,7 @@
   },
   {
     "name": "RobotBase Skeleton (Advanced)",
-    "description": "Skeleton (stub) code for RobotBase",
+    "description": "Skeleton (stub) code for RobotBase - Not recommended for competition use",
     "tags": [
       "RobotBase",
       "Skeleton"

wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp

@@ -0,0 +1,152 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <thread>
+
+#include <frc/Preferences.h>
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/JoystickSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <hal/simulation/MockHooks.h>
+#include <units/length.h>
+#include <units/time.h>
+
+#include "Constants.h"
+#include "Robot.h"
+
+class ArmSimulationTest : public testing::TestWithParam<units::degree_t> {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+
+ protected:
+  frc::sim::PWMSim m_motorSim{kMotorPort};
+  frc::sim::EncoderSim m_encoderSim =
+      frc::sim::EncoderSim::CreateForChannel(kEncoderAChannel);
+  frc::sim::JoystickSim m_joystickSim{kJoystickPort};
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    m_encoderSim.ResetData();
+    m_motorSim.ResetData();
+    frc::sim::DriverStationSim::ResetData();
+    frc::Preferences::RemoveAll();
+  }
+};
+
+TEST_P(ArmSimulationTest, Teleop) {
+  EXPECT_TRUE(frc::Preferences::ContainsKey(kArmPositionKey));
+  EXPECT_TRUE(frc::Preferences::ContainsKey(kArmPKey));
+  EXPECT_DOUBLE_EQ(kDefaultArmSetpoint.value(),
+                   frc::Preferences::GetDouble(kArmPositionKey, NAN));
+
+  frc::Preferences::SetDouble(kArmPositionKey, GetParam().value());
+  units::degree_t setpoint = GetParam();
+  // teleop init
+  {
+    frc::sim::DriverStationSim::SetAutonomous(false);
+    frc::sim::DriverStationSim::SetEnabled(true);
+    frc::sim::DriverStationSim::NotifyNewData();
+
+    EXPECT_TRUE(m_motorSim.GetInitialized());
+    EXPECT_TRUE(m_encoderSim.GetInitialized());
+  }
+
+  {
+    frc::sim::StepTiming(3_s);
+
+    // Ensure elevator is still at 0.
+    EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 2.0);
+  }
+
+  {
+    // Press button to reach setpoint
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    frc::sim::StepTiming(1.5_s);
+
+    EXPECT_NEAR(setpoint.value(),
+                units::radian_t(m_encoderSim.GetDistance())
+                    .convert<units::degree>()
+                    .value(),
+                2.0);
+
+    // see setpoint is held.
+    frc::sim::StepTiming(0.5_s);
+
+    EXPECT_NEAR(setpoint.value(),
+                units::radian_t(m_encoderSim.GetDistance())
+                    .convert<units::degree>()
+                    .value(),
+                2.0);
+  }
+
+  {
+    // Unpress the button to go back down
+    m_joystickSim.SetTrigger(false);
+    m_joystickSim.NotifyNewData();
+
+    frc::sim::StepTiming(3_s);
+
+    EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 2.0);
+  }
+
+  {
+    // Press button to go back up
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    // advance 75 timesteps
+    frc::sim::StepTiming(1.5_s);
+
+    EXPECT_NEAR(setpoint.value(),
+                units::radian_t(m_encoderSim.GetDistance())
+                    .convert<units::degree>()
+                    .value(),
+                2.0);
+
+    // advance 25 timesteps to see setpoint is held.
+    frc::sim::StepTiming(0.5_s);
+
+    EXPECT_NEAR(setpoint.value(),
+                units::radian_t(m_encoderSim.GetDistance())
+                    .convert<units::degree>()
+                    .value(),
+                2.0);
+  }
+
+  {
+    // Disable
+    frc::sim::DriverStationSim::SetAutonomous(false);
+    frc::sim::DriverStationSim::SetEnabled(false);
+    frc::sim::DriverStationSim::NotifyNewData();
+
+    frc::sim::StepTiming(3_s);
+
+    ASSERT_NEAR(0.0, m_motorSim.GetSpeed(), 0.05);
+    EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 2.0);
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    ArmSimulationTests, ArmSimulationTest,
+    testing::Values(kDefaultArmSetpoint, 25.0_deg, 50.0_deg),
+    [](const testing::TestParamInfo<units::degree_t>& info) {
+      return testing::PrintToString(info.param.value())
+          .append(std::string(info.param.abbreviation()));
+    });

wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/main.cpp

@@ -0,0 +1,17 @@
+// 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.
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp

@@ -0,0 +1,126 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/JoystickSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <hal/simulation/MockHooks.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/time.h>
+
+#include "Constants.h"
+#include "Robot.h"
+
+using namespace Constants;
+
+class ElevatorSimulationTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+
+ protected:
+  frc::sim::PWMSim m_motorSim{Constants::kMotorPort};
+  frc::sim::EncoderSim m_encoderSim =
+      frc::sim::EncoderSim::CreateForChannel(Constants::kEncoderAChannel);
+  frc::sim::JoystickSim m_joystickSim{Constants::kJoystickPort};
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    m_encoderSim.ResetData();
+    m_motorSim.ResetData();
+    frc::sim::DriverStationSim::ResetData();
+  }
+};
+
+TEST_F(ElevatorSimulationTest, Teleop) {
+  // teleop init
+  {
+    frc::sim::DriverStationSim::SetAutonomous(false);
+    frc::sim::DriverStationSim::SetEnabled(true);
+    frc::sim::DriverStationSim::NotifyNewData();
+
+    EXPECT_TRUE(m_motorSim.GetInitialized());
+    EXPECT_TRUE(m_encoderSim.GetInitialized());
+  }
+
+  {
+    // advance 50 timesteps
+    frc::sim::StepTiming(1_s);
+
+    // Ensure elevator is still at 0.
+    EXPECT_NEAR(0.0, m_encoderSim.GetDistance(), 0.05);
+  }
+
+  {
+    // Press button to reach setpoint
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    // advance 75 timesteps
+    frc::sim::StepTiming(1.5_s);
+
+    EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
+
+    // advance 25 timesteps to see setpoint is held.
+    frc::sim::StepTiming(0.5_s);
+
+    EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
+  }
+
+  {
+    // Unpress the button to go back down
+    m_joystickSim.SetTrigger(false);
+    m_joystickSim.NotifyNewData();
+
+    // advance 75 timesteps
+    frc::sim::StepTiming(1.5_s);
+
+    EXPECT_NEAR(0.0, m_encoderSim.GetDistance(), 0.05);
+  }
+
+  {
+    // Press button to go back up
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    // advance 75 timesteps
+    frc::sim::StepTiming(1.5_s);
+
+    EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
+
+    // advance 25 timesteps to see setpoint is held.
+    frc::sim::StepTiming(0.5_s);
+
+    EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
+  }
+
+  {
+    // Disable
+    frc::sim::DriverStationSim::SetAutonomous(false);
+    frc::sim::DriverStationSim::SetEnabled(false);
+    frc::sim::DriverStationSim::NotifyNewData();
+
+    // advance 75 timesteps
+    frc::sim::StepTiming(1.5_s);
+
+    ASSERT_NEAR(0.0, m_motorSim.GetSpeed(), 0.05);
+    ASSERT_NEAR(0.0, m_encoderSim.GetDistance(), 0.05);
+  }
+}

wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/main.cpp

@@ -0,0 +1,17 @@
+// 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.
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java

@@ -277,9 +277,9 @@ public class PneumaticHub implements PneumaticsBase {
    * below {@code minPressure} and will turn off when the pressure reaches {@code maxPressure}.
    *
    * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
-   *     drops below this value.
+   *     drops below this value. Range 0-120 PSI.
    * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
-   *     reaches this value.
+   *     reaches this value. Range 0-120 PSI. Must be larger then minPressure.
    */
   @Override
   public void enableCompressorAnalog(double minPressure, double maxPressure) {
@@ -294,6 +294,10 @@ public class PneumaticHub implements PneumaticsBase {
       throw new IllegalArgumentException(
           "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
     }
+
+    // Send the voltage as it would be if the 5V rail was at exactly 5V.
+    // The firmware will compensate for the real 5V rail voltage, which
+    // can fluctuate somewhat over time.
     double minAnalogVoltage = psiToVolts(minPressure, 5);
     double maxAnalogVoltage = psiToVolts(maxPressure, 5);
     REVPHJNI.setClosedLoopControlAnalog(m_handle, minAnalogVoltage, maxAnalogVoltage);
@@ -320,10 +324,11 @@ public class PneumaticHub implements PneumaticsBase {
    * </ul>
    *
    * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure
-   *     drops below this value and the pressure switch indicates that the system is not full.
+   *     drops below this value and the pressure switch indicates that the system is not full. Range
+   *     0-120 PSI.
    * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure
    *     reaches this value or the pressure switch is disconnected or indicates that the system is
-   *     full.
+   *     full. Range 0-120 PSI. Must be larger then minPressure.
    */
   @Override
   public void enableCompressorHybrid(double minPressure, double maxPressure) {
@@ -338,6 +343,10 @@ public class PneumaticHub implements PneumaticsBase {
       throw new IllegalArgumentException(
           "maxPressure must be between 0 and 120 PSI, got " + maxPressure);
     }
+
+    // Send the voltage as it would be if the 5V rail was at exactly 5V.
+    // The firmware will compensate for the real 5V rail voltage, which
+    // can fluctuate somewhat over time.
     double minAnalogVoltage = psiToVolts(minPressure, 5);
     double maxAnalogVoltage = psiToVolts(maxPressure, 5);
     REVPHJNI.setClosedLoopControlHybrid(m_handle, minAnalogVoltage, maxAnalogVoltage);

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -16,6 +16,7 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.networktables.MultiSubscriber;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.util.WPILibVersion;
@@ -128,6 +129,11 @@ public abstract class RobotBase implements AutoCloseable {
                 break;
             }
           }
+
+          @Override
+          public double getTimestamp() {
+            return WPIUtilJNI.now() * 1.0e-6;
+          }
         });
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java

@@ -18,8 +18,16 @@ public class DutyCycleEncoderSim {
    * @param encoder DutyCycleEncoder to simulate
    */
   public DutyCycleEncoderSim(DutyCycleEncoder encoder) {
-    SimDeviceSim wrappedSimDevice =
-        new SimDeviceSim("DutyCycle:DutyCycleEncoder" + "[" + encoder.getSourceChannel() + "]");
+    this(encoder.getSourceChannel());
+  }
+
+  /**
+   * Constructs from a digital input channel.
+   *
+   * @param channel digital input channel.
+   */
+  public DutyCycleEncoderSim(int channel) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycle:DutyCycleEncoder", channel);
     m_simPosition = wrappedSimDevice.getDouble("position");
     m_simDistancePerRotation = wrappedSimDevice.getDouble("distance_per_rot");
   }

wpilibj/src/test/java/edu/wpi/first/wpilibj/NotifierTest.java

@@ -0,0 +1,63 @@
+// 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.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+/** Tests to see if the Notifier is working properly. */
+class NotifierTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    SimHooks.restartTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testStartPeriodicAndStop() {
+    AtomicInteger counter = new AtomicInteger();
+    Notifier notifier = new Notifier(counter::getAndIncrement);
+    notifier.startPeriodic(1.0);
+
+    SimHooks.stepTiming(10);
+
+    notifier.stop();
+    assertEquals(10, counter.get());
+
+    SimHooks.stepTiming(3.0);
+
+    assertEquals(10, counter.get());
+
+    notifier.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testStartSingle() {
+    AtomicInteger counter = new AtomicInteger();
+    Notifier notifier = new Notifier(counter::getAndIncrement);
+    notifier.startSingle(1.0);
+
+    SimHooks.stepTiming(10.5);
+
+    assertEquals(1, counter.get());
+
+    notifier.close();
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java

@@ -21,6 +21,8 @@ import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.ValueSource;
 
 class TimedRobotTest {
+  static final double kPeriod = 0.02;
+
   static class MockRobot extends TimedRobot {
     public final AtomicInteger m_robotInitCount = new AtomicInteger(0);
     public final AtomicInteger m_simulationInitCount = new AtomicInteger(0);
@@ -41,6 +43,10 @@ class TimedRobotTest {
     public final AtomicInteger m_teleopExitCount = new AtomicInteger(0);
     public final AtomicInteger m_testExitCount = new AtomicInteger(0);
 
+    MockRobot() {
+      super(kPeriod);
+    }
+
     @Override
     public void robotInit() {
       m_robotInitCount.addAndGet(1);
@@ -168,7 +174,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -189,7 +195,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -257,7 +263,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -278,7 +284,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -346,7 +352,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -367,7 +373,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -438,7 +444,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -459,7 +465,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -549,7 +555,7 @@ class TimedRobotTest {
     assertEquals(0, robot.m_teleopExitCount.get());
     assertEquals(0, robot.m_testExitCount.get());
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(0, robot.m_autonomousInitCount.get());
@@ -567,7 +573,7 @@ class TimedRobotTest {
     DriverStationSim.setTest(false);
     DriverStationSim.notifyNewData();
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_autonomousInitCount.get());
@@ -585,7 +591,7 @@ class TimedRobotTest {
     DriverStationSim.setTest(false);
     DriverStationSim.notifyNewData();
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_autonomousInitCount.get());
@@ -603,7 +609,7 @@ class TimedRobotTest {
     DriverStationSim.setTest(true);
     DriverStationSim.notifyNewData();
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_autonomousInitCount.get());
@@ -621,7 +627,7 @@ class TimedRobotTest {
     DriverStationSim.setTest(false);
     DriverStationSim.notifyNewData();
 
-    SimHooks.stepTiming(0.02);
+    SimHooks.stepTiming(kPeriod);
 
     assertEquals(2, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_autonomousInitCount.get());
@@ -653,7 +659,7 @@ class TimedRobotTest {
         () -> {
           callbackCount.addAndGet(1);
         },
-        0.01);
+        kPeriod / 2.0);
 
     Thread robotThread =
         new Thread(
@@ -670,13 +676,13 @@ class TimedRobotTest {
     assertEquals(0, robot.m_disabledPeriodicCount.get());
     assertEquals(0, callbackCount.get());
 
-    SimHooks.stepTiming(0.01);
+    SimHooks.stepTiming(kPeriod / 2.0);
 
     assertEquals(0, robot.m_disabledInitCount.get());
     assertEquals(0, robot.m_disabledPeriodicCount.get());
     assertEquals(1, callbackCount.get());
 
-    SimHooks.stepTiming(0.01);
+    SimHooks.stepTiming(kPeriod / 2.0);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_disabledPeriodicCount.get());
@@ -702,15 +708,17 @@ class TimedRobotTest {
         () -> {
           callbackCount.addAndGet(1);
         },
-        0.01,
-        0.005);
+        kPeriod / 2.0,
+        kPeriod / 4.0);
 
     // Expirations in this test (ms)
     //
+    // Let p be period in ms.
+    //
     // Robot | Callback
     // ================
-    //    20 |      15
-    //    40 |      25
+    //     p |    0.75p
+    //    2p |    1.25p
 
     Thread robotThread =
         new Thread(
@@ -727,25 +735,25 @@ class TimedRobotTest {
     assertEquals(0, robot.m_disabledPeriodicCount.get());
     assertEquals(0, callbackCount.get());
 
-    SimHooks.stepTiming(0.0075);
+    SimHooks.stepTiming(kPeriod * 3.0 / 8.0);
 
     assertEquals(0, robot.m_disabledInitCount.get());
     assertEquals(0, robot.m_disabledPeriodicCount.get());
     assertEquals(0, callbackCount.get());
 
-    SimHooks.stepTiming(0.0075);
+    SimHooks.stepTiming(kPeriod * 3.0 / 8.0);
 
     assertEquals(0, robot.m_disabledInitCount.get());
     assertEquals(0, robot.m_disabledPeriodicCount.get());
     assertEquals(1, callbackCount.get());
 
-    SimHooks.stepTiming(0.005);
+    SimHooks.stepTiming(kPeriod / 4.0);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_disabledPeriodicCount.get());
     assertEquals(1, callbackCount.get());
 
-    SimHooks.stepTiming(0.005);
+    SimHooks.stepTiming(kPeriod / 4.0);
 
     assertEquals(1, robot.m_disabledInitCount.get());
     assertEquals(1, robot.m_disabledPeriodicCount.get());

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java

@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final String kArmPositionKey = "ArmPosition";
+  public static final String kArmPKey = "ArmP";
+
+  // The P gain for the PID controller that drives this arm.
+  public static final double kDefaultArmKp = 50.0;
+  public static final double kDefaultArmSetpointDegrees = 75.0;
+
+  // distance per pulse = (angle per revolution) / (pulses per revolution)
+  //  = (2 * PI rads) / (4096 pulses)
+  public static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
+
+  public static final double kArmReduction = 200;
+  public static final double kArmMass = 8.0; // Kilograms
+  public static final double kArmLength = Units.inchesToMeters(30);
+  public static final double kMinAngleRads = Units.degreesToRadians(-75);
+  public static final double kMaxAngleRads = Units.degreesToRadians(255);
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java

@@ -4,150 +4,48 @@
 
 package edu.wpi.first.wpilibj.examples.armsimulation;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Preferences;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj.util.Color8Bit;
+import edu.wpi.first.wpilibj.examples.armsimulation.subsystems.Arm;
 
 /** This is a sample program to demonstrate the use of arm simulation with existing code. */
 public class Robot extends TimedRobot {
-  private static final int kMotorPort = 0;
-  private static final int kEncoderAChannel = 0;
-  private static final int kEncoderBChannel = 1;
-  private static final int kJoystickPort = 0;
-
-  public static final String kArmPositionKey = "ArmPosition";
-  public static final String kArmPKey = "ArmP";
-
-  // The P gain for the PID controller that drives this arm.
-  private static double kArmKp = 50.0;
-
-  private static double armPositionDeg = 75.0;
-
-  // distance per pulse = (angle per revolution) / (pulses per revolution)
-  //  = (2 * PI rads) / (4096 pulses)
-  private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
-
-  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
-  private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
-
-  // Standard classes for controlling our arm
-  private final PIDController m_controller = new PIDController(kArmKp, 0, 0);
-  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
-  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
-  private final Joystick m_joystick = new Joystick(kJoystickPort);
-
-  // Simulation classes help us simulate what's going on, including gravity.
-  private static final double m_armReduction = 200;
-  private static final double m_armMass = 8.0; // Kilograms
-  private static final double m_armLength = Units.inchesToMeters(30);
-  // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
-  // to 255 degrees (rotated down in the back).
-  private final SingleJointedArmSim m_armSim =
-      new SingleJointedArmSim(
-          m_armGearbox,
-          m_armReduction,
-          SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
-          m_armLength,
-          Units.degreesToRadians(-75),
-          Units.degreesToRadians(255),
-          true,
-          VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
-          );
-  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
-
-  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
-  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
-  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
-  private final MechanismLigament2d m_armTower =
-      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
-  private final MechanismLigament2d m_arm =
-      m_armPivot.append(
-          new MechanismLigament2d(
-              "Arm",
-              30,
-              Units.radiansToDegrees(m_armSim.getAngleRads()),
-              6,
-              new Color8Bit(Color.kYellow)));
+  private final Arm m_arm = new Arm();
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
 
   @Override
-  public void robotInit() {
-    m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse);
-
-    // Put Mechanism 2d to SmartDashboard
-    SmartDashboard.putData("Arm Sim", m_mech2d);
-    m_armTower.setColor(new Color8Bit(Color.kBlue));
-
-    // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
-    if (!Preferences.containsKey(kArmPositionKey)) {
-      Preferences.setDouble(kArmPositionKey, armPositionDeg);
-    }
-    if (!Preferences.containsKey(kArmPKey)) {
-      Preferences.setDouble(kArmPKey, kArmKp);
-    }
-  }
+  public void robotInit() {}
 
   @Override
   public void simulationPeriodic() {
-    // In this method, we update our simulation of what our arm is doing
-    // First, we set our "inputs" (voltages)
-    m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
-
-    // Next, we update it. The standard loop time is 20ms.
-    m_armSim.update(0.020);
-
-    // Finally, we set our simulated encoder's readings and simulated battery voltage
-    m_encoderSim.setDistance(m_armSim.getAngleRads());
-    // SimBattery estimates loaded battery voltages
-    RoboRioSim.setVInVoltage(
-        BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
-
-    // Update the Mechanism Arm angle based on the simulated arm angle
-    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
+    m_arm.simulationPeriodic();
   }
 
   @Override
   public void teleopInit() {
-    // Read Preferences for Arm setpoint and kP on entering Teleop
-    armPositionDeg = Preferences.getDouble(kArmPositionKey, armPositionDeg);
-    if (kArmKp != Preferences.getDouble(kArmPKey, kArmKp)) {
-      kArmKp = Preferences.getDouble(kArmPKey, kArmKp);
-      m_controller.setP(kArmKp);
-    }
+    m_arm.loadPreferences();
   }
 
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 75 degrees.
-      var pidOutput =
-          m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(armPositionDeg));
-      m_motor.setVoltage(pidOutput);
+      // Here, we run PID control like normal.
+      m_arm.reachSetpoint();
     } else {
       // Otherwise, we disable the motor.
-      m_motor.set(0.0);
+      m_arm.stop();
     }
   }
 
+  @Override
+  public void close() {
+    m_arm.close();
+    super.close();
+  }
+
   @Override
   public void disabledInit() {
     // This just makes sure that our simulation code knows that the motor's off.
-    m_motor.set(0.0);
+    m_arm.stop();
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java

@@ -0,0 +1,134 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation.subsystems;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.armsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Arm implements AutoCloseable {
+  // The P gain for the PID controller that drives this arm.
+  private double m_armKp = Constants.kDefaultArmKp;
+  private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees;
+
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
+  private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
+
+  // Standard classes for controlling our arm
+  private final PIDController m_controller = new PIDController(m_armKp, 0, 0);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+  // to 255 degrees (rotated down in the back).
+  private final SingleJointedArmSim m_armSim =
+      new SingleJointedArmSim(
+          m_armGearbox,
+          Constants.kArmReduction,
+          SingleJointedArmSim.estimateMOI(Constants.kArmLength, Constants.kArmMass),
+          Constants.kArmLength,
+          Constants.kMinAngleRads,
+          Constants.kMaxAngleRads,
+          true,
+          VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
+          );
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+  private final MechanismLigament2d m_armTower =
+      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+  private final MechanismLigament2d m_arm =
+      m_armPivot.append(
+          new MechanismLigament2d(
+              "Arm",
+              30,
+              Units.radiansToDegrees(m_armSim.getAngleRads()),
+              6,
+              new Color8Bit(Color.kYellow)));
+
+  /** Subsystem constructor. */
+  public Arm() {
+    m_encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    SmartDashboard.putData("Arm Sim", m_mech2d);
+    m_armTower.setColor(new Color8Bit(Color.kBlue));
+
+    // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
+    Preferences.initDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
+    Preferences.initDouble(Constants.kArmPKey, m_armKp);
+  }
+
+  /** Update the simulation model. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our arm is doing
+    // First, we set our "inputs" (voltages)
+    m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_armSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_armSim.getAngleRads());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
+  }
+
+  /** Load setpoint and kP from preferences. */
+  public void loadPreferences() {
+    // Read Preferences for Arm setpoint and kP on entering Teleop
+    m_armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
+    if (m_armKp != Preferences.getDouble(Constants.kArmPKey, m_armKp)) {
+      m_armKp = Preferences.getDouble(Constants.kArmPKey, m_armKp);
+      m_controller.setP(m_armKp);
+    }
+  }
+
+  /** Run the control loop to reach and maintain the setpoint from the preferences. */
+  public void reachSetpoint() {
+    var pidOutput =
+        m_controller.calculate(
+            m_encoder.getDistance(), Units.degreesToRadians(m_armSetpointDegrees));
+    m_motor.setVoltage(pidOutput);
+  }
+
+  public void stop() {
+    m_motor.set(0.0);
+  }
+
+  @Override
+  public void close() {
+    m_motor.close();
+    m_encoder.close();
+    m_mech2d.close();
+    m_armPivot.close();
+    m_controller.close();
+    m_arm.close();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java

@@ -0,0 +1,37 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final double kElevatorKp = 5;
+  public static final double kElevatorKi = 0;
+  public static final double kElevatorKd = 0;
+
+  public static final double kElevatorkS = 0.0; // volts (V)
+  public static final double kElevatorkG = 0.762; // volts (V)
+  public static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
+  public static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
+
+  public static final double kElevatorGearing = 10.0;
+  public static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
+  public static final double kCarriageMass = 4.0; // kg
+
+  public static final double kSetpointMeters = 0.75;
+  // Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+  public static final double kMinElevatorHeightMeters = 0.0;
+  public static final double kMaxElevatorHeightMeters = 1.25;
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  public static final double kElevatorEncoderDistPerPulse =
+      2.0 * Math.PI * kElevatorDrumRadius / 4096;
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java

@@ -4,137 +4,50 @@
 
 package edu.wpi.first.wpilibj.examples.elevatorsimulation;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
-import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems.Elevator;
 
 /** This is a sample program to demonstrate the use of elevator simulation. */
 public class Robot extends TimedRobot {
-  private static final int kMotorPort = 0;
-  private static final int kEncoderAChannel = 0;
-  private static final int kEncoderBChannel = 1;
-  private static final int kJoystickPort = 0;
-
-  private static final double kElevatorKp = 5;
-  private static final double kElevatorKi = 0;
-  private static final double kElevatorKd = 0;
-
-  private static final double kElevatorkS = 0.0; // volts (V)
-  private static final double kElevatorkG = 0.762; // volts (V)
-  private static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
-  private static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
-
-  private static final double kElevatorGearing = 10.0;
-  private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
-  private static final double kCarriageMass = 4.0; // kg
-
-  private static final double kSetpoint = Units.inchesToMeters(30);
-  private static final double kMinElevatorHeight = Units.inchesToMeters(2);
-  private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
-
-  // distance per pulse = (distance per revolution) / (pulses per revolution)
-  //  = (Pi * D) / ppr
-  private static final double kElevatorEncoderDistPerPulse =
-      2.0 * Math.PI * kElevatorDrumRadius / 4096;
-
-  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
-  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
-
-  // Standard classes for controlling our elevator
-  private final ProfiledPIDController m_controller =
-      new ProfiledPIDController(
-          kElevatorKp, kElevatorKi, kElevatorKd, new TrapezoidProfile.Constraints(2.45, 2.45));
-  ElevatorFeedforward m_feedforward =
-      new ElevatorFeedforward(kElevatorkS, kElevatorkG, kElevatorkV, kElevatorkA);
-  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
-  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
-  private final Joystick m_joystick = new Joystick(kJoystickPort);
-
-  // Simulation classes help us simulate what's going on, including gravity.
-  private final ElevatorSim m_elevatorSim =
-      new ElevatorSim(
-          m_elevatorGearbox,
-          kElevatorGearing,
-          kCarriageMass,
-          kElevatorDrumRadius,
-          kMinElevatorHeight,
-          kMaxElevatorHeight,
-          true,
-          VecBuilder.fill(0.01));
-  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
-
-  // Create a Mechanism2d visualization of the elevator
-  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
-  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
-  private final MechanismLigament2d m_elevatorMech2d =
-      m_mech2dRoot.append(
-          new MechanismLigament2d(
-              "Elevator", Units.metersToInches(m_elevatorSim.getPositionMeters()), 90));
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
+  private final Elevator m_elevator = new Elevator();
 
   @Override
-  public void robotInit() {
-    m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
+  public void robotInit() {}
 
-    // Publish Mechanism2d to SmartDashboard
-    // To view the Elevator Sim in the simulator, select Network Tables -> SmartDashboard ->
-    // Elevator Sim
-    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  @Override
+  public void robotPeriodic() {
+    // Update the telemetry, including mechanism visualization, regardless of mode.
+    m_elevator.updateTelemetry();
   }
 
   @Override
   public void simulationPeriodic() {
-    // In this method, we update our simulation of what our elevator is doing
-    // First, we set our "inputs" (voltages)
-    m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
-
-    // Next, we update it. The standard loop time is 20ms.
-    m_elevatorSim.update(0.020);
-
-    // Finally, we set our simulated encoder's readings and simulated battery voltage
-    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
-    // SimBattery estimates loaded battery voltages
-    RoboRioSim.setVInVoltage(
-        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
-
-    // Update elevator visualization with simulated position
-    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
+    // Update the simulation model.
+    m_elevator.simulationPeriodic();
   }
 
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we set the constant setpoint of 30in.
-      m_controller.setGoal(kSetpoint);
+      // Here, we set the constant setpoint of 0.75 meters.
+      m_elevator.reachGoal(Constants.kSetpointMeters);
     } else {
       // Otherwise, we update the setpoint to 0.
-      m_controller.setGoal(0.0);
+      m_elevator.reachGoal(0.0);
     }
-    // With the setpoint value we run PID control like normal
-    double pidOutput = m_controller.calculate(m_encoder.getDistance());
-    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
-    m_motor.setVoltage(pidOutput + feedforwardOutput);
   }
-  // To view the Elevator in the simulator, select Network Tables -> SmartDashboard -> Elevator Sim
 
   @Override
   public void disabledInit() {
     // This just makes sure that our simulation code knows that the motor's off.
-    m_motor.set(0.0);
+    m_elevator.stop();
+  }
+
+  @Override
+  public void close() {
+    m_elevator.close();
+    super.close();
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java

@@ -0,0 +1,125 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.elevatorsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Elevator implements AutoCloseable {
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(
+          Constants.kElevatorKp,
+          Constants.kElevatorKi,
+          Constants.kElevatorKd,
+          new TrapezoidProfile.Constraints(2.45, 2.45));
+  ElevatorFeedforward m_feedforward =
+      new ElevatorFeedforward(
+          Constants.kElevatorkS,
+          Constants.kElevatorkG,
+          Constants.kElevatorkV,
+          Constants.kElevatorkA);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private final ElevatorSim m_elevatorSim =
+      new ElevatorSim(
+          m_elevatorGearbox,
+          Constants.kElevatorGearing,
+          Constants.kCarriageMass,
+          Constants.kElevatorDrumRadius,
+          Constants.kMinElevatorHeightMeters,
+          Constants.kMaxElevatorHeightMeters,
+          true,
+          VecBuilder.fill(0.01));
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+  private final PWMSim m_motorSim = new PWMSim(m_motor);
+
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
+  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(
+          new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90));
+
+  /** Subsystem constructor. */
+  public Elevator() {
+    m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    // To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  }
+
+  /** Advance the simulation. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+  }
+
+  /**
+   * Run control loop to reach and maintain goal.
+   *
+   * @param goal the position to maintain
+   */
+  public void reachGoal(double goal) {
+    m_controller.setGoal(goal);
+
+    // With the setpoint value we run PID control like normal
+    double pidOutput = m_controller.calculate(m_encoder.getDistance());
+    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
+    m_motor.setVoltage(pidOutput + feedforwardOutput);
+  }
+
+  /** Stop the control loop and motor output. */
+  public void stop() {
+    m_controller.setGoal(0.0);
+    m_motor.set(0.0);
+  }
+
+  /** Update telemetry, including the mechanism visualization. */
+  public void updateTelemetry() {
+    // Update elevator visualization with position
+    m_elevatorMech2d.setLength(m_encoder.getDistance());
+  }
+
+  @Override
+  public void close() {
+    m_encoder.close();
+    m_motor.close();
+    m_mech2d.close();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -1,7 +1,7 @@
 [
   {
     "name": "Getting Started",
-    "description": "An example program which demonstrates the simplest autonomous and teleoperated routines.",
+    "description": "A differential-drive robot with split-stick Xbox arcade drive with a simple time-based autonomous.",
     "tags": [
       "Basic Robot"
     ],
@@ -12,7 +12,7 @@
   },
   {
     "name": "Tank Drive",
-    "description": "Demonstrate the use of the DifferentialDrive class doing teleop driving with tank steering",
+    "description": "Control a differential drive with twin-joystick tank drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -25,7 +25,7 @@
   },
   {
     "name": "Arcade Drive",
-    "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Arcade Drive.",
+    "description": "Control a differential drivetrain with single-joystick arcade drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -38,7 +38,7 @@
   },
   {
     "name": "Mecanum Drive",
-    "description": "Demonstrate the use of the MecanumDrive class doing teleop driving with Mecanum steering",
+    "description": "Control a mecanum drivetrain with a joystick in teleop.",
     "tags": [
       "Basic Robot",
       "Mecanum Drive",
@@ -51,7 +51,7 @@
   },
   {
     "name": "PDP CAN Monitoring",
-    "description": "Demonstrate using CAN to monitor the voltage, current, and temperature in the Power Distribution Panel.",
+    "description": "Monitor Power Distribution data such as voltage, current, temperature, etc.",
     "tags": [
       "Hardware",
       "PDP",
@@ -64,7 +64,7 @@
   },
   {
     "name": "Solenoids",
-    "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.",
+    "description": "Control a single and double solenoid from joystick buttons.",
     "tags": [
       "Hardware",
       "Joystick",
@@ -77,7 +77,7 @@
   },
   {
     "name": "Encoder",
-    "description": "Demonstrate displaying the value of a quadrature encoder on the SmartDashboard.",
+    "description": "View values from a quadrature encoder.",
     "tags": [
       "Hardware",
       "Encoder",
@@ -90,7 +90,7 @@
   },
   {
     "name": "EventLoop",
-    "description": "Demonstrate managing a ball system using EventLoop and BooleanEvent.",
+    "description": "Manage a ball system using EventLoop and BooleanEvent.",
     "tags": [
       "Basic Robot",
       "Flywheel",
@@ -103,7 +103,7 @@
   },
   {
     "name": "Relay",
-    "description": "Demonstrate controlling a Relay from Joystick buttons.",
+    "description": "Control a relay from joystick buttons.",
     "tags": [
       "Hardware",
       "Relay",
@@ -116,7 +116,7 @@
   },
   {
     "name": "Ultrasonic",
-    "description": "Demonstrate using the Ultrasonic class with a ping-response ultrasonic sensor.",
+    "description": "View values from a ping-response ultrasonic sensor.",
     "tags": [
       "Hardware",
       "Ultrasonic",
@@ -130,7 +130,7 @@
   },
   {
     "name": "Ultrasonic PID",
-    "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.",
+    "description": "Maintain a set distance from an obstacle with an ultrasonic sensor and PID control.",
     "tags": [
       "Basic Robot",
       "Ultrasonic",
@@ -144,7 +144,7 @@
   },
   {
     "name": "Potentiometer PID",
-    "description": "An example to demonstrate the use of a potentiometer and PID control to maintain elevator position setpoints.",
+    "description": "Maintain elevator position setpoints with a potentiometer and PID control.",
     "tags": [
       "Basic Robot",
       "Analog",
@@ -159,7 +159,7 @@
   },
   {
     "name": "Elevator with trapezoid profiled PID",
-    "description": "Demonstrate the use of trapezoid profiles with a smart motor controller's PID to reach elevator position setpoints.",
+    "description": "Reach elevator position setpoints with trapezoid profiles and smart motor controller PID.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -174,7 +174,7 @@
   },
   {
     "name": "Elevator with profiled PID controller",
-    "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+    "description": "Reach elevator position setpoints with an encoder and profiled PID control.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -188,7 +188,7 @@
   },
   {
     "name": "Gyro",
-    "description": "An example program showing how to drive straight using a gyro sensor.",
+    "description": "Drive a differential drive straight with a gyro sensor.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -204,7 +204,7 @@
   },
   {
     "name": "Gyro Mecanum",
-    "description": "An example program showing how to perform mecanum drive with field oriented controls.",
+    "description": "Drive a mecanum drivetrain using field-oriented controls with a joystick.",
     "tags": [
       "Basic Robot",
       "Mecanum Drive",
@@ -219,7 +219,7 @@
   },
   {
     "name": "HID Rumble",
-    "description": "An example program showing how to make human interface devices rumble.",
+    "description": "Make human interface devices (HID) rumble.",
     "tags": [
       "Hardware",
       "XboxController"
@@ -231,7 +231,7 @@
   },
   {
     "name": "Mechanism2d",
-    "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "description": "Display mechanism states on a dashboard with Mechanism2d.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -248,7 +248,7 @@
   },
   {
     "name": "Motor Control",
-    "description": "Demonstrate controlling a single motor with a Joystick and displaying the net movement of the motor using an encoder.",
+    "description": "Control a single motor with a joystick, displaying the movement of the motor using an encoder.",
     "tags": [
       "Basic Robot",
       "Encoder",
@@ -262,7 +262,7 @@
   },
   {
     "name": "GearsBot",
-    "description": "A fully functional example CommandBased program for WPIs GearsBot robot, ported to the new CommandBased library. This code can run on your computer if it supports simulation.",
+    "description": "A fully functional Command-Based program for WPI's GearsBot robot.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -281,7 +281,7 @@
   },
   {
     "name": "Simple Vision",
-    "description": "Demonstrate the use of the CameraServer class to stream from a USB Webcam without processing the images.",
+    "description": "Use the CameraServer class to stream from a USB Webcam without processing the images.",
     "tags": [
       "Vision"
     ],
@@ -292,7 +292,7 @@
   },
   {
     "name": "Intermediate Vision",
-    "description": "An example program that acquires images from an attached USB camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display.",
+    "description": "Acquire images from an attached USB camera and add some annotation to the image (as you might do for showing operators the result of some image recognition) and send it to the dashboard for display.",
     "tags": [
       "Vision"
     ],
@@ -303,7 +303,7 @@
   },
   {
     "name": "Axis Camera Sample",
-    "description": "An example program that acquires images from an Axis network camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display. This demonstrates the use of the AxisCamera class.",
+    "description": "Acquire images from an Axis network camera and adds some annotation to the image (as you might do for showing operators the result of some image recognition), and sends it to the dashboard for display.",
     "tags": [
       "Vision"
     ],
@@ -314,7 +314,7 @@
   },
   {
     "name": "AprilTags Vision",
-    "description": "on-roboRIO detection of AprilTags using an attached USB camera.",
+    "description": "On-roboRIO detection of AprilTags using an attached USB camera.",
     "tags": [
       "Vision",
       "AprilTags"
@@ -325,8 +325,8 @@
     "commandversion": 2
   },
   {
-    "name": "Shuffleboard Sample",
-    "description": "An example program that adds data to various Shuffleboard tabs that demonstrates the Shuffleboard API",
+    "name": "Shuffleboard",
+    "description": "Present various data via the Shuffleboard API.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -342,7 +342,7 @@
   },
   {
     "name": "'Traditional' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game, written in the 'traditional' style, i.e. commands are given their own classes.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -361,7 +361,7 @@
   },
   {
     "name": "'Inlined' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -380,7 +380,7 @@
   },
   {
     "name": "Rapid React Command Bot",
-    "description": "A fully-functional command-based fender bot for the 2022 game using the new command framework.",
+    "description": "A fully-functional command-based fender bot for the 2022 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -400,7 +400,7 @@
   },
   {
     "name": "Select Command Example",
-    "description": "An example showing how to use the SelectCommand class from the new command framework.",
+    "description": "Use SelectCommand to select an autonomous routine.",
     "tags": [
       "Command-based"
     ],
@@ -411,7 +411,7 @@
   },
   {
     "name": "Frisbeebot",
-    "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework",
+    "description": "A simple frisbee shooter for the 2013 game, demonstrating use of PIDSubsystem.",
     "tags": [
       "Complete Robot",
       "Command-based",
@@ -428,7 +428,7 @@
   },
   {
     "name": "Gyro Drive Commands",
-    "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.",
+    "description": "Control a robot's angle with PID and a gyro, in command-based.",
     "tags": [
       "Command-based",
       "Differential Drive",
@@ -445,7 +445,7 @@
   },
   {
     "name": "SwerveBot",
-    "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.",
+    "description": "Use kinematics and odometry with a swerve drive.",
     "tags": [
       "Swerve Drive",
       "Odometry",
@@ -460,7 +460,7 @@
   },
   {
     "name": "MecanumBot",
-    "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.",
+    "description": "Use kinematics and odometry with a mecanum drive.",
     "tags": [
       "Mecanum Drive",
       "Odometry",
@@ -475,7 +475,7 @@
   },
   {
     "name": "DifferentialDriveBot",
-    "description": "An example program for a differential drive that uses differential drive kinematics and odometry.",
+    "description": "Use kinematics and odometry with a differential drive.",
     "tags": [
       "Differential Drive",
       "Odometry",
@@ -490,7 +490,7 @@
   },
   {
     "name": "RamseteCommand",
-    "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
+    "description": "Follow a pre-generated trajectory with a differential drive using RamseteCommand.",
     "tags": [
       "Differential Drive",
       "Command-based",
@@ -509,7 +509,7 @@
   },
   {
     "name": "Arcade Drive Xbox Controller",
-    "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Arcade Drive and an Xbox Controller",
+    "description": "Control a differential drive with split-stick arcade drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -522,7 +522,7 @@
   },
   {
     "name": "Tank Drive Xbox Controller",
-    "description": "Demonstrates the use of the DifferentialDrive class to drive a robot with Tank Drive and an Xbox Controller",
+    "description": "Control a differential drive with Xbox tank drive in teleop.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -535,7 +535,7 @@
   },
   {
     "name": "Duty Cycle Encoder",
-    "description": "Demonstrates the use of the Duty Cycle Encoder class",
+    "description": "View values from a duty-cycle encoder.",
     "tags": [
       "Hardware",
       "Duty Cycle",
@@ -549,7 +549,7 @@
   },
   {
     "name": "Duty Cycle Input",
-    "description": "Demonstrates the use of the Duty Cycle class",
+    "description": "View duty-cycle input.",
     "tags": [
       "Hardware",
       "Duty Cycle",
@@ -562,7 +562,7 @@
   },
   {
     "name": "Addressable LED",
-    "description": "Demonstrates the use of the Addressable LED class",
+    "description": "Display a rainbow pattern on an addressable LED strip.",
     "tags": [
       "Hardware",
       "Basic Robot",
@@ -575,7 +575,7 @@
   },
   {
     "name": "DMA",
-    "description": "Demonstrates the use of the DMA class",
+    "description": "Read various sensors using DMA.",
     "tags": [
       "Hardware",
       "DMA",
@@ -588,7 +588,7 @@
   },
   {
     "name": "ArmBot",
-    "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+    "description": "Control an arm with ProfiledPIDSubsystem.",
     "tags": [
       "Command-based",
       "Arm",
@@ -604,7 +604,7 @@
   },
   {
     "name": "ArmBotOffboard",
-    "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+    "description": "Control an arm with TrapezoidProfileSubsystem and smart motor controller PID.",
     "tags": [
       "Command-based",
       "Arm",
@@ -620,7 +620,7 @@
   },
   {
     "name": "DriveDistanceOffboard",
-    "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+    "description": "Drive a differential drivetrain a set distance using TrapezoidProfileCommand and smart motor controller PID.",
     "tags": [
       "Command-based",
       "Differential Drive",
@@ -635,7 +635,7 @@
   },
   {
     "name": "MecanumControllerCommand",
-    "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+    "description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
     "tags": [
       "Command-based",
       "Mecanum Drive",
@@ -653,7 +653,7 @@
   },
   {
     "name": "SwerveControllerCommand",
-    "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+    "description": "Follow a pre-generated trajectory with a swerve drive using SwerveControllerCommand.",
     "tags": [
       "Command-based",
       "Swerve Drive",
@@ -671,7 +671,7 @@
   },
   {
     "name": "RamseteController",
-    "description": "An example robot demonstrating the use of RamseteController.",
+    "description": "Follow a pre-generated trajectory with a differential drive using RamseteController.",
     "tags": [
       "Basic Robot",
       "Differential Drive",
@@ -689,7 +689,7 @@
   },
   {
     "name": "StateSpaceFlywheel",
-    "description": "An example state-space controller for a flywheel.",
+    "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Flywheel",
@@ -705,7 +705,7 @@
   },
   {
     "name": "StateSpaceFlywheelSysId",
-    "description": "An example state-space controller for controlling a flywheel with System Identification.",
+    "description": "Control a flywheel using a state-space model (based on values from SysId), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Flywheel",
@@ -722,7 +722,7 @@
   },
   {
     "name": "StateSpaceElevator",
-    "description": "An example state-space controller for controlling an elevator.",
+    "description": "Control an elevator using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -738,7 +738,7 @@
   },
   {
     "name": "StateSpaceArm",
-    "description": "An example state-space controller for controlling an arm.",
+    "description": "Control an arm using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
     "tags": [
       "Basic Robot",
       "Arm",
@@ -754,7 +754,7 @@
   },
   {
     "name": "SimpleDifferentialDriveSimulation",
-    "description": "An example of a minimal drivetrain simulation project without the command-based library.",
+    "description": "Simulate a differential drivetrain and follow trajectories with RamseteController (non-command-based).",
     "tags": [
       "Differential Drive",
       "State-Space",
@@ -772,7 +772,7 @@
   },
   {
     "name": "StateSpaceDriveSimulation",
-    "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.",
+    "description": "Simulate a differential drivetrain and follow trajectories with RamseteCommand (command-based).",
     "tags": [
       "Command-based",
       "Differential Drive",
@@ -787,7 +787,7 @@
   },
   {
     "name": "ElevatorSimulation",
-    "description": "Demonstrates the use of physics simulation with a simple elevator.",
+    "description": "Simulate an elevator.",
     "tags": [
       "Basic Robot",
       "Elevator",
@@ -803,7 +803,7 @@
   },
   {
     "name": "ArmSimulation",
-    "description": "Demonstrates the use of physics simulation with a simple single-jointedarm.",
+    "description": "Simulate a single-jointed arm.",
     "tags": [
       "Basic Robot",
       "Arm",
@@ -819,7 +819,7 @@
   },
   {
     "name": "UnitTesting",
-    "description": "Demonstrates basic unit testing for a robot project.",
+    "description": "Test a robot project with basic unit tests in simulation.",
     "tags": [
       "Intake",
       "Pneumatics"
@@ -831,7 +831,7 @@
   },
   {
     "name": "DifferentialDrivePoseEstimator",
-    "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+    "description": "Combine differential-drive odometry with vision data using DifferentialDrivePoseEstimator.",
     "tags": [
       "Differential Drive",
       "State-Space",
@@ -847,7 +847,7 @@
   },
   {
     "name": "MecanumDrivePoseEstimator",
-    "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum drive odometry.",
+    "description": "Combine mecanum-drive odometry with vision data using MecanumDrivePoseEstimator.",
     "tags": [
       "Mecanum Drive",
       "State-Space",
@@ -863,7 +863,7 @@
   },
   {
     "name": "SwerveDrivePoseEstimator",
-    "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for swerve drive odometry.",
+    "description": "Combine swerve-drive odometry with vision data using SwerveDrivePoseEstimator.",
     "tags": [
       "Swerve Drive",
       "State-Space",
@@ -894,7 +894,7 @@
   },
   {
     "name": "Digital Communication Sample",
-    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's DIO",
+    "description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",
     "tags": [
       "Hardware",
       "Digital Output"
@@ -906,7 +906,7 @@
   },
   {
     "name": "I2C Communication Sample",
-    "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's I2C port",
+    "description": "Communicate with external devices (such as an Arduino) using the roboRIO's I2C port.",
     "tags": [
       "Hardware",
       "I2C"

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java

@@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -18,7 +17,7 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 public class Robot extends TimedRobot {
   static final int kPotChannel = 1;
   static final int kMotorChannel = 7;
-  static final int kJoystickChannel = 0;
+  static final int kJoystickChannel = 3;
 
   // The elevator can move 1.5 meters from top to bottom
   static final double kFullHeightMeters = 1.5;
@@ -37,7 +36,7 @@ public class Robot extends TimedRobot {
   // Scaling is handled internally
   private final AnalogPotentiometer m_potentiometer =
       new AnalogPotentiometer(kPotChannel, kFullHeightMeters);
-  private final MotorController m_elevatorMotor = new PWMSparkMax(kMotorChannel);
+  private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(kMotorChannel);
   private final Joystick m_joystick = new Joystick(kJoystickChannel);
 
   private int m_index;
@@ -64,7 +63,17 @@ public class Robot extends TimedRobot {
     if (m_joystick.getTriggerPressed()) {
       // index of the elevator setpoint wraps around.
       m_index = (m_index + 1) % kSetpointsMeters.length;
+      System.out.println("m_index = " + m_index);
       m_pidController.setSetpoint(kSetpointsMeters[m_index]);
     }
   }
+
+  @Override
+  public void close() {
+    m_elevatorMotor.close();
+    m_potentiometer.close();
+    m_pidController.close();
+    m_index = 0;
+    super.close();
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json

@@ -1,7 +1,7 @@
 [
   {
     "name": "Command Robot",
-    "description": "Command style",
+    "description": "Command-based, with explanatory comments and example code.",
     "tags": [
       "Command"
     ],
@@ -12,7 +12,7 @@
   },
   {
     "name": "Command Robot Skeleton (Advanced)",
-    "description": "Skeleton (stub) code for Command Robot",
+    "description": "Skeleton (stub) code for Command-based, without explanatory comments and example code.",
     "tags": [
       "Command",
       "Skeleton"
@@ -24,7 +24,7 @@
   },
   {
     "name": "Timed Robot",
-    "description": "Timed style",
+    "description": "Timed style, with explanatory comments and example code.",
     "tags": [
       "Timed"
     ],
@@ -35,7 +35,7 @@
   },
   {
     "name": "Timed Skeleton (Advanced)",
-    "description": "Skeleton (stub) code for TimedRobot",
+    "description": "Skeleton (stub) code for TimedRobot, without explanatory comments and example code.",
     "tags": [
       "Timed",
       "Skeleton"
@@ -47,7 +47,7 @@
   },
   {
     "name": "RobotBase Skeleton (Advanced)",
-    "description": "Skeleton (stub) code for RobotBase",
+    "description": "Skeleton (stub) code for RobotBase - Not recommended for competition use",
     "tags": [
       "RobotBase",
       "Skeleton"

wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java

@@ -0,0 +1,153 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+@ResourceLock("timing")
+class ArmSimulationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+
+  private PWMSim m_motorSim;
+  private EncoderSim m_encoderSim;
+  private JoystickSim m_joystickSim;
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_encoderSim = EncoderSim.createForChannel(Constants.kEncoderAChannel);
+    m_motorSim = new PWMSim(Constants.kMotorPort);
+    m_joystickSim = new JoystickSim(Constants.kJoystickPort);
+
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_encoderSim.resetData();
+    m_motorSim.resetData();
+    Preferences.remove(Constants.kArmPKey);
+    Preferences.remove(Constants.kArmPositionKey);
+    Preferences.removeAll();
+    RoboRioSim.resetData();
+    DriverStationSim.resetData();
+    DriverStationSim.notifyNewData();
+  }
+
+  @ValueSource(doubles = {Constants.kDefaultArmSetpointDegrees, 25.0, 50.0})
+  @ParameterizedTest
+  void teleopTest(double setpoint) {
+    assertTrue(Preferences.containsKey(Constants.kArmPositionKey));
+    assertTrue(Preferences.containsKey(Constants.kArmPKey));
+    assertEquals(
+        Constants.kDefaultArmSetpointDegrees,
+        Preferences.getDouble(Constants.kArmPositionKey, Double.NaN));
+
+    Preferences.setDouble(Constants.kArmPositionKey, setpoint);
+    // teleop init
+    {
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+
+      assertTrue(m_motorSim.getInitialized());
+      assertTrue(m_encoderSim.getInitialized());
+    }
+
+    {
+      // advance 50 timesteps
+      SimHooks.stepTiming(3);
+
+      // Ensure elevator is still at 0.
+      assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
+    }
+
+    {
+      // Press button to reach setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+    }
+
+    {
+      // Unpress the button to go back down
+      m_joystickSim.setTrigger(false);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(3.0);
+
+      assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
+    }
+
+    {
+      // Press button to go back up
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+    }
+
+    {
+      // Disable
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(false);
+      DriverStationSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(3.5);
+
+      assertEquals(0.0, m_motorSim.getSpeed(), 0.01);
+      assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
+    }
+  }
+}

wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java

@@ -0,0 +1,139 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+@ResourceLock("timing")
+class ElevatorSimulationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+
+  private PWMSim m_motorSim;
+  private EncoderSim m_encoderSim;
+  private JoystickSim m_joystickSim;
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_encoderSim = EncoderSim.createForChannel(Constants.kEncoderAChannel);
+    m_motorSim = new PWMSim(Constants.kMotorPort);
+    m_joystickSim = new JoystickSim(Constants.kJoystickPort);
+
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_encoderSim.resetData();
+    m_motorSim.resetData();
+    RoboRioSim.resetData();
+    DriverStationSim.resetData();
+    DriverStationSim.notifyNewData();
+  }
+
+  @Test
+  void teleopTest() {
+    // teleop init
+    {
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+
+      assertTrue(m_motorSim.getInitialized());
+      assertTrue(m_encoderSim.getInitialized());
+    }
+
+    {
+      // advance 50 timesteps
+      SimHooks.stepTiming(1);
+
+      // Ensure elevator is still at 0.
+      assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Press button to reach setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Unpress the button to go back down
+      m_joystickSim.setTrigger(false);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Press button to go back up
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Disable
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(false);
+      DriverStationSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(0.0, m_motorSim.getSpeed(), 0.05);
+      assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/MathShared.java

@@ -20,4 +20,11 @@ public interface MathShared {
    * @param count the usage count
    */
   void reportUsage(MathUsageId id, int count);
+
+  /**
+   * Get the current time.
+   *
+   * @return Time in seconds
+   */
+  double getTimestamp();
 }

wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.util.WPIUtilJNI;
+
 public final class MathSharedStore {
   private static MathShared mathShared;
 
@@ -23,6 +25,11 @@ public final class MathSharedStore {
 
             @Override
             public void reportUsage(MathUsageId id, int count) {}
+
+            @Override
+            public double getTimestamp() {
+              return WPIUtilJNI.now() * 1.0e-6;
+            }
           };
     }
     return mathShared;
@@ -56,4 +63,13 @@ public final class MathSharedStore {
   public static void reportUsage(MathUsageId id, int count) {
     getMathShared().reportUsage(id, count);
   }
+
+  /**
+   * Get the time.
+   *
+   * @return The time in seconds.
+   */
+  public static double getTimestamp() {
+    return getMathShared().getTimestamp();
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -17,8 +18,8 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Map;
+import java.util.NoSuchElementException;
 import java.util.Objects;
 
 /**
@@ -190,7 +191,11 @@ public class DifferentialDrivePoseEstimator {
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
-    if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+    try {
+      if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+        return;
+      }
+    } catch (NoSuchElementException ex) {
       return;
     }
 
@@ -287,7 +292,7 @@ public class DifferentialDrivePoseEstimator {
   public Pose2d update(
       Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
     return updateWithTime(
-        WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
+        MathSharedStore.getTimestamp(), gyroAngle, distanceLeftMeters, distanceRightMeters);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -18,8 +19,8 @@ import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Map;
+import java.util.NoSuchElementException;
 import java.util.Objects;
 
 /**
@@ -178,7 +179,11 @@ public class MecanumDrivePoseEstimator {
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
-    if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+    try {
+      if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+        return;
+      }
+    } catch (NoSuchElementException ex) {
       return;
     }
 
@@ -264,7 +269,7 @@ public class MecanumDrivePoseEstimator {
    * @return The estimated pose of the robot in meters.
    */
   public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
+    return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -18,9 +19,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
 import java.util.Map;
+import java.util.NoSuchElementException;
 import java.util.Objects;
 
 /**
@@ -180,7 +181,11 @@ public class SwerveDrivePoseEstimator {
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
-    if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+    try {
+      if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+        return;
+      }
+    } catch (NoSuchElementException ex) {
       return;
     }
 
@@ -266,7 +271,7 @@ public class SwerveDrivePoseEstimator {
    * @return The estimated pose of the robot in meters.
    */
   public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, modulePositions);
+    return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, modulePositions);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.filter;
 
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.math.MathSharedStore;
 
 /**
  * A simple debounce filter for boolean streams. Requires that the boolean change value from
@@ -60,11 +60,11 @@ public class Debouncer {
   }
 
   private void resetTimer() {
-    m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6;
+    m_prevTimeSeconds = MathSharedStore.getTimestamp();
   }
 
   private boolean hasElapsed() {
-    return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds;
+    return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds;
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.math.filter;
 
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -33,7 +33,7 @@ public class SlewRateLimiter {
     m_positiveRateLimit = positiveRateLimit;
     m_negativeRateLimit = negativeRateLimit;
     m_prevVal = initialValue;
-    m_prevTime = WPIUtilJNI.now() * 1e-6;
+    m_prevTime = MathSharedStore.getTimestamp();
   }
 
   /**
@@ -67,7 +67,7 @@ public class SlewRateLimiter {
    * @return The filtered value, which will not change faster than the slew rate.
    */
   public double calculate(double input) {
-    double currentTime = WPIUtilJNI.now() * 1e-6;
+    double currentTime = MathSharedStore.getTimestamp();
     double elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         MathUtil.clamp(
@@ -85,6 +85,6 @@ public class SlewRateLimiter {
    */
   public void reset(double value) {
     m_prevVal = value;
-    m_prevTime = WPIUtilJNI.now() * 1e-6;
+    m_prevTime = MathSharedStore.getTimestamp();
   }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -205,18 +205,25 @@ public class Pose3d implements Interpolatable<Pose3d> {
     final var u = VecBuilder.fill(twist.dx, twist.dy, twist.dz);
     final var rvec = VecBuilder.fill(twist.rx, twist.ry, twist.rz);
     final var omega = rotationVectorToMatrix(rvec);
-    final var omegaSq = omega.pow(2);
+    final var omegaSq = omega.times(omega);
     double theta = rvec.norm();
     double thetaSq = theta * theta;
 
     double A;
     double B;
     double C;
-    if (theta < 1E-9) {
+    if (Math.abs(theta) < 1E-9) {
       // Taylor Expansions around θ = 0
       // A = 1/1! - θ²/3! + θ⁴/5!
       // B = 1/2! - θ²/4! + θ⁴/6!
       // C = 1/3! - θ²/5! + θ⁴/7!
+      // sources:
+      // A:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+      // B:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      // C:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
       A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
       B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
       C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
@@ -257,16 +264,23 @@ public class Pose3d implements Interpolatable<Pose3d> {
     final var rvec = transform.getRotation().getQuaternion().toRotationVector();
 
     final var omega = rotationVectorToMatrix(rvec);
-    final var theta = transform.getRotation().getAngle();
+    final var theta = rvec.norm();
     final var thetaSq = theta * theta;
 
     double C;
-    if (theta < 1E-9) {
+    if (Math.abs(theta) < 1E-9) {
       // Taylor Expansions around θ = 0
       // A = 1/1! - θ²/3! + θ⁴/5!
       // B = 1/2! - θ²/4! + θ⁴/6!
       // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
-      C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
+      // sources:
+      // A:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+      // B:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      // C:
+      // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+      C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
     } else {
       // A = sin(θ)/θ
       // B = (1 - cos(θ)) / θ²
@@ -276,7 +290,8 @@ public class Pose3d implements Interpolatable<Pose3d> {
       C = (1 - A / (2 * B)) / thetaSq;
     }
 
-    final var V_inv = Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omega.pow(2).times(C));
+    final var V_inv =
+        Matrix.eye(Nat.N3()).minus(omega.times(0.5)).plus(omega.times(omega).times(C));
 
     final var twist_translation =
         V_inv.times(VecBuilder.fill(transform.getX(), transform.getY(), transform.getZ()));

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -72,6 +72,17 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
             cr * cp * sy - sr * sp * cy);
   }
 
+  /**
+   * Constructs a Rotation3d with the given rotation vector representation. This representation is
+   * equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the
+   * axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  public Rotation3d(Vector<N3> rvec) {
+    this(rvec, rvec.norm());
+  }
+
   /**
    * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
    * normalized.

wpimath/src/main/native/cpp/MathShared.cpp

@@ -5,6 +5,9 @@
 #include "wpimath/MathShared.h"
 
 #include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
 
 using namespace wpi::math;
 
@@ -15,6 +18,9 @@ class DefaultMathShared : public MathShared {
   void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
   }
   void ReportUsage(MathUsageId id, int count) override {}
+  units::second_t GetTimestamp() override {
+    return units::second_t{wpi::Now() * 1.0e-6};
+  }
 };
 }  // namespace
 

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -8,6 +8,7 @@
 
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
@@ -96,8 +97,9 @@ void DifferentialDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
   // Step 0: If this measurement is old enough to be outside the pose buffer's
   // timespan, skip.
-  if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
-      timestamp) {
+  if (!m_poseBuffer.GetInternalBuffer().empty() &&
+      m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+          timestamp) {
     return;
   }
 
@@ -152,7 +154,7 @@ void DifferentialDrivePoseEstimator::AddVisionMeasurement(
 Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
                                               units::meter_t leftDistance,
                                               units::meter_t rightDistance) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+  return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                         leftDistance, rightDistance);
 }
 

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -8,6 +8,7 @@
 
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
@@ -107,8 +108,9 @@ void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
   // Step 0: If this measurement is old enough to be outside the pose buffer's
   // timespan, skip.
-  if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
-      timestamp) {
+  if (!m_poseBuffer.GetInternalBuffer().empty() &&
+      m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+          timestamp) {
     return;
   }
 
@@ -162,7 +164,7 @@ void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
 Pose2d frc::MecanumDrivePoseEstimator::Update(
     const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+  return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                         wheelPositions);
 }
 

wpimath/src/main/native/cpp/filter/Debouncer.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/filter/Debouncer.h"
 
+#include "wpimath/MathShared.h"
+
 using namespace frc;
 
 Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
@@ -21,11 +23,12 @@ Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
 }
 
 void Debouncer::ResetTimer() {
-  m_prevTime = units::microsecond_t(wpi::Now());
+  m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
 }
 
 bool Debouncer::HasElapsed() const {
-  return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime;
+  return wpi::math::MathSharedStore::GetTimestamp() - m_prevTime >=
+         m_debounceTime;
 }
 
 bool Debouncer::Calculate(bool input) {

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -72,21 +72,28 @@ Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
 
 Pose3d Pose3d::Exp(const Twist3d& twist) const {
   // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
-  auto u = Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
-  auto rvec = Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()};
-  auto omega = RotationVectorToMatrix(rvec);
-  auto omegaSq = omega * omega;
-  auto theta = rvec.norm();
-  auto thetaSq = theta * theta;
+  Eigen::Vector3d u{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+  Eigen::Vector3d rvec{twist.rx.value(), twist.ry.value(), twist.rz.value()};
+  Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+  Eigen::Matrix3d omegaSq = omega * omega;
+  double theta = rvec.norm();
+  double thetaSq = theta * theta;
 
   double A;
   double B;
   double C;
-  if (theta < 1E-9) {
+  if (std::abs(theta) < 1E-9) {
     // Taylor Expansions around θ = 0
     // A = 1/1! - θ²/3! + θ⁴/5!
     // B = 1/2! - θ²/4! + θ⁴/6!
     // C = 1/3! - θ²/5! + θ⁴/7!
+    // sources:
+    // A:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+    // B:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    // C:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
     A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
     B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
     C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
@@ -99,8 +106,8 @@ Pose3d Pose3d::Exp(const Twist3d& twist) const {
     C = (1 - A) / thetaSq;
   }
 
-  auto R = Matrixd<3, 3>::Identity() + A * omega + B * omegaSq;
-  auto V = Matrixd<3, 3>::Identity() + B * omega + C * omegaSq;
+  Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + A * omega + B * omegaSq;
+  Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + B * omega + C * omegaSq;
 
   auto translation_component = V * u;
   const Transform3d transform{
@@ -116,22 +123,30 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
   // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
   const auto transform = end.RelativeTo(*this);
 
-  auto u = Vectord<3>{transform.X().value(), transform.Y().value(),
-                      transform.Z().value()};
-  auto rvec = transform.Rotation().GetQuaternion().ToRotationVector();
+  Eigen::Vector3d u{transform.X().value(), transform.Y().value(),
+                    transform.Z().value()};
+  Eigen::Vector3d rvec =
+      transform.Rotation().GetQuaternion().ToRotationVector();
 
-  auto omega = RotationVectorToMatrix(rvec);
-  auto omegaSq = omega * omega;
-  auto theta = rvec.norm();
-  auto thetaSq = theta * theta;
+  Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+  Eigen::Matrix3d omegaSq = omega * omega;
+  double theta = rvec.norm();
+  double thetaSq = theta * theta;
 
   double C;
-  if (theta < 1E-9) {
+  if (std::abs(theta) < 1E-9) {
     // Taylor Expansions around θ = 0
     // A = 1/1! - θ²/3! + θ⁴/5!
     // B = 1/2! - θ²/4! + θ⁴/6!
     // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
-    C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
+    // sources:
+    // A:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+    // B:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    // C:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
   } else {
     // A = std::sin(θ)/θ
     // B = (1 - std::cos(θ)) / θ²
@@ -141,9 +156,10 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
     C = (1 - A / (2 * B)) / thetaSq;
   }
 
-  auto V_inv = Matrixd<3, 3>::Identity() - 0.5 * omega + C * omegaSq;
+  Eigen::Matrix3d V_inv =
+      Eigen::Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
 
-  auto translation_component = V_inv * u;
+  Eigen::Vector3d translation_component = V_inv * u;
 
   return Twist3d{units::meter_t{translation_component(0)},
                  units::meter_t{translation_component(1)},

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -38,6 +38,9 @@ Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
                    cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
 }
 
+Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
+    : Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
+
 Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
   double norm = axis.norm();
   if (norm == 0.0) {

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -18,6 +18,7 @@
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 
@@ -172,8 +173,9 @@ class SwerveDrivePoseEstimator {
                             units::second_t timestamp) {
     // Step 0: If this measurement is old enough to be outside the pose buffer's
     // timespan, skip.
-    if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
-        timestamp) {
+    if (!m_poseBuffer.GetInternalBuffer().empty() &&
+        m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+            timestamp) {
       return;
     }
 
@@ -271,7 +273,7 @@ class SwerveDrivePoseEstimator {
   Pose2d Update(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+    return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                           modulePositions);
   }
 

wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h

@@ -10,6 +10,7 @@
 #include <wpi/timestamp.h>
 
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -45,7 +46,8 @@ class SlewRateLimiter {
       : m_positiveRateLimit{positiveRateLimit},
         m_negativeRateLimit{negativeRateLimit},
         m_prevVal{initialValue},
-        m_prevTime{units::microsecond_t(wpi::Now())} {}
+        m_prevTime{
+            units::microsecond_t(wpi::math::MathSharedStore::GetTimestamp())} {}
 
   /**
    * Creates a new SlewRateLimiter with the given positive rate limit and
@@ -77,7 +79,7 @@ class SlewRateLimiter {
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    units::second_t currentTime = units::microsecond_t(wpi::Now());
+    units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
     units::second_t elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
@@ -94,7 +96,7 @@ class SlewRateLimiter {
    */
   void Reset(Unit_t value) {
     m_prevVal = value;
-    m_prevTime = units::microsecond_t(wpi::Now());
+    m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
   }
 
  private:

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -59,13 +59,22 @@ class WPILIB_DLLEXPORT Rotation3d {
    */
   Rotation3d(const Vectord<3>& axis, units::radian_t angle);
 
+  /**
+   * Constructs a Rotation3d with the given rotation vector representation. This
+   * representation is equivalent to axis-angle, where the normalized axis is
+   * multiplied by the rotation around the axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  explicit Rotation3d(const Eigen::Vector3d& rvec);
+
   /**
    * Constructs a Rotation3d from a rotation matrix.
    *
    * @param rotationMatrix The rotation matrix.
    * @throws std::domain_error if the rotation matrix isn't special orthogonal.
    */
-  explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+  explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix);
 
   /**
    * Constructs a Rotation3d that rotates the initial vector onto the final

wpimath/src/main/native/include/wpimath/MathShared.h

@@ -9,6 +9,8 @@
 #include <fmt/format.h>
 #include <wpi/SymbolExports.h>
 
+#include "units/time.h"
+
 namespace wpi::math {
 
 enum class MathUsageId {
@@ -31,6 +33,7 @@ class WPILIB_DLLEXPORT MathShared {
   virtual void ReportWarningV(fmt::string_view format,
                               fmt::format_args args) = 0;
   virtual void ReportUsage(MathUsageId id, int count) = 0;
+  virtual units::second_t GetTimestamp() = 0;
 
   template <typename S, typename... Args>
   inline void ReportError(const S& format, Args&&... args) {
@@ -70,6 +73,10 @@ class WPILIB_DLLEXPORT MathSharedStore {
   static void ReportUsage(MathUsageId id, int count) {
     GetMathShared().ReportUsage(id, count);
   }
+
+  static units::second_t GetTimestamp() {
+    return GetMathShared().GetTimestamp();
+  }
 };
 
 }  // namespace wpi::math

wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java

@@ -24,17 +24,23 @@ class Rotation3dTest {
     final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
     final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
     final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
+    final var rvec1 = new Rotation3d(xAxis.times(Math.PI / 3));
     assertEquals(rot1, rot2);
+    assertEquals(rot1, rvec1);
 
     final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
     final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
     final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
+    final var rvec2 = new Rotation3d(yAxis.times(Math.PI / 3));
     assertEquals(rot3, rot4);
+    assertEquals(rot3, rvec2);
 
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
     final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
     final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
+    final var rvec3 = new Rotation3d(zAxis.times(Math.PI / 3));
     assertEquals(rot5, rot6);
+    assertEquals(rot5, rvec3);
   }
 
   @Test

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -17,17 +17,23 @@ TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot2{units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
+  const Rotation3d rvec1{Eigen::Vector3d{xAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot1, rot2);
+  EXPECT_EQ(rot1, rvec1);
 
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
   const Rotation3d rot3{yAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot4{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+  const Rotation3d rvec2{Eigen::Vector3d{yAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot3, rot4);
+  EXPECT_EQ(rot3, rvec2);
 
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
   const Rotation3d rot5{zAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot6{0_rad, 0_rad, units::radian_t{std::numbers::pi / 3}};
+  const Rotation3d rvec3{Eigen::Vector3d{zAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot5, rot6);
+  EXPECT_EQ(rot5, rvec3);
 }
 
 TEST(Rotation3dTest, InitRotationMatrix) {

wpinet/src/main/native/cpp/DsClient.cpp

@@ -21,6 +21,9 @@ DsClient::DsClient(wpi::uv::Loop& loop, wpi::Logger& logger,
     : m_logger{logger},
       m_tcp{uv::Tcp::Create(loop)},
       m_timer{uv::Timer::Create(loop)} {
+  if (!m_tcp || !m_timer) {
+    return;
+  }
   m_tcp->end.connect([this] {
     WPI_DEBUG4(m_logger, "DS connection closed");
     clearIp();

wpinet/src/main/native/cpp/EventLoopRunner.cpp

@@ -59,6 +59,7 @@ void EventLoopRunner::Stop() {
       h.SetLoopClosing(true);
       h.Close();
     });
+    loop.SetClosing();
   });
   m_owner.Join();
 }

wpinet/src/main/native/cpp/ParallelTcpConnector.cpp

@@ -24,6 +24,9 @@ ParallelTcpConnector::ParallelTcpConnector(
       m_reconnectRate{reconnectRate},
       m_connected{std::move(connected)},
       m_reconnectTimer{uv::Timer::Create(loop)} {
+  if (!m_reconnectTimer) {
+    return;
+  }
   m_reconnectTimer->timeout.connect([this] {
     if (!IsConnected()) {
       WPI_DEBUG1(m_logger, "timed out, reconnecting");
@@ -86,6 +89,9 @@ void ParallelTcpConnector::Connect() {
           // kick off parallel connection attempts
           for (auto ai = &addrinfo; ai; ai = ai->ai_next) {
             auto tcp = uv::Tcp::Create(m_loop);
+            if (!tcp) {
+              continue;
+            }
             m_attempts.emplace_back(tcp);
 
             auto connreq = std::make_shared<uv::TcpConnectReq>();

wpinet/src/main/native/cpp/PortForwarder.cpp

@@ -49,6 +49,9 @@ void PortForwarder::Add(unsigned int port, std::string_view remoteHost,
                         unsigned int remotePort) {
   m_impl->runner.ExecSync([&](uv::Loop& loop) {
     auto server = uv::Tcp::Create(loop);
+    if (!server) {
+      return;
+    }
 
     // bind to local port
     server->Bind("", port);
@@ -71,6 +74,10 @@ void PortForwarder::Add(unsigned int port, std::string_view remoteHost,
       client->SetData(connected);
 
       auto remote = uv::Tcp::Create(loop);
+      if (!remote) {
+        client->Close();
+        return;
+      }
       remote->error.connect(
           [remotePtr = remote.get(),
            clientWeak = std::weak_ptr<uv::Tcp>(client)](uv::Error err) {

wpinet/src/main/native/cpp/WebSocket.cpp

@@ -37,6 +37,9 @@ class WebSocketWriteReq : public uv::WriteReq {
   std::function<void(std::span<uv::Buffer>, uv::Error)> m_callback;
   SmallVector<uv::Buffer, 4> m_internalBufs;
   SmallVector<uv::Buffer, 4> m_userBufs;
+
+  // for server
+  size_t m_internalBufPos = 0;
 };
 }  // namespace
 
@@ -126,7 +129,6 @@ void WebSocket::Close(uint16_t code, std::string_view reason) {
   SendClose(code, reason);
   if (m_state != FAILED && m_state != CLOSED) {
     m_state = CLOSING;
-    closed(code, reason);
   }
 }
 
@@ -259,11 +261,12 @@ void WebSocket::StartClient(std::string_view uri, std::string_view host,
 
   // Start handshake timer if a timeout was specified
   if (options.handshakeTimeout != (uv::Timer::Time::max)()) {
-    auto timer = uv::Timer::Create(m_stream.GetLoopRef());
-    timer->timeout.connect(
-        [this]() { Terminate(1006, "connection timed out"); });
-    timer->Start(options.handshakeTimeout);
-    m_clientHandshake->timer = timer;
+    if (auto timer = uv::Timer::Create(m_stream.GetLoopRef())) {
+      timer->timeout.connect(
+          [this]() { Terminate(1006, "connection timed out"); });
+      timer->Start(options.handshakeTimeout);
+      m_clientHandshake->timer = timer;
+    }
   }
 }
 
@@ -338,11 +341,8 @@ void WebSocket::SetClosed(uint16_t code, std::string_view reason, bool failed) {
   if (m_state == FAILED || m_state == CLOSED) {
     return;
   }
-  bool wasClosing = m_state == CLOSING;
   m_state = failed ? FAILED : CLOSED;
-  if (!wasClosing) {
-    closed(code, reason);
-  }
+  closed(code, reason);
 }
 
 void WebSocket::Shutdown() {
@@ -601,9 +601,6 @@ void WebSocket::HandleIncoming(uv::Buffer& buf, size_t size) {
 static void WriteFrame(WebSocketWriteReq& req,
                        SmallVectorImpl<uv::Buffer>& bufs, bool server,
                        uint8_t opcode, std::span<const uv::Buffer> data) {
-  SmallVector<uv::Buffer, 4> internalBufs;
-  raw_uv_ostream os{internalBufs, 4096};
-
 #ifdef WPINET_WEBSOCKET_VERBOSE_DEBUG
   if ((opcode & 0x7f) == 0x01) {
     SmallString<128> str;
@@ -623,8 +620,11 @@ static void WriteFrame(WebSocketWriteReq& req,
   }
 #endif
 
+  uint8_t header[10];
+  uint8_t* pHeader = header;
+
   // opcode (includes FIN bit)
-  os << static_cast<unsigned char>(opcode);
+  *pHeader++ = opcode;
 
   // payload length
   uint64_t size = 0;
@@ -632,27 +632,30 @@ static void WriteFrame(WebSocketWriteReq& req,
     size += buf.len;
   }
   if (size < 126) {
-    os << static_cast<unsigned char>((server ? 0x00 : kFlagMasking) | size);
+    *pHeader++ = (server ? 0x00 : kFlagMasking) | size;
   } else if (size <= 0xffff) {
-    os << static_cast<unsigned char>((server ? 0x00 : kFlagMasking) | 126);
-    const uint8_t sizeMsb[] = {static_cast<uint8_t>((size >> 8) & 0xff),
-                               static_cast<uint8_t>(size & 0xff)};
-    os << std::span{sizeMsb};
+    *pHeader++ = (server ? 0x00 : kFlagMasking) | 126;
+    *pHeader++ = (size >> 8) & 0xff;
+    *pHeader++ = size & 0xff;
   } else {
-    os << static_cast<unsigned char>((server ? 0x00 : kFlagMasking) | 127);
-    const uint8_t sizeMsb[] = {static_cast<uint8_t>((size >> 56) & 0xff),
-                               static_cast<uint8_t>((size >> 48) & 0xff),
-                               static_cast<uint8_t>((size >> 40) & 0xff),
-                               static_cast<uint8_t>((size >> 32) & 0xff),
-                               static_cast<uint8_t>((size >> 24) & 0xff),
-                               static_cast<uint8_t>((size >> 16) & 0xff),
-                               static_cast<uint8_t>((size >> 8) & 0xff),
-                               static_cast<uint8_t>(size & 0xff)};
-    os << std::span{sizeMsb};
+    *pHeader++ = (server ? 0x00 : kFlagMasking) | 127;
+    *pHeader++ = (size >> 56) & 0xff;
+    *pHeader++ = (size >> 48) & 0xff;
+    *pHeader++ = (size >> 40) & 0xff;
+    *pHeader++ = (size >> 32) & 0xff;
+    *pHeader++ = (size >> 24) & 0xff;
+    *pHeader++ = (size >> 16) & 0xff;
+    *pHeader++ = (size >> 8) & 0xff;
+    *pHeader++ = size & 0xff;
   }
+  size_t headerSize = pHeader - header;
 
   // clients need to mask the input data
   if (!server) {
+    SmallVector<uv::Buffer, 4> internalBufs;
+    raw_uv_ostream os{internalBufs, 4096};
+
+    os << std::span<const uint8_t>{header, headerSize};
     // generate masking key
     static std::random_device rd;
     static std::default_random_engine gen{rd()};
@@ -673,13 +676,23 @@ static void WriteFrame(WebSocketWriteReq& req,
       }
     }
     bufs.append(internalBufs.begin(), internalBufs.end());
+    req.m_internalBufs.append(internalBufs.begin(), internalBufs.end());
     // don't send the user bufs as we copied their data
   } else {
-    bufs.append(internalBufs.begin(), internalBufs.end());
+    // manage m_internalBufs to efficiently store header
+    if (req.m_internalBufs.empty() ||
+        (req.m_internalBufPos + headerSize) > 4096) {
+      req.m_internalBufs.emplace_back(uv::Buffer::Allocate(4096));
+      req.m_internalBufPos = 0;
+    }
+    char* internalBuf =
+        req.m_internalBufs.back().data().data() + req.m_internalBufPos;
+    std::memcpy(internalBuf, header, headerSize);
+    bufs.emplace_back(internalBuf, headerSize);
+    req.m_internalBufPos += headerSize;
     // servers can just send the buffers directly without masking
     bufs.append(data.begin(), data.end());
   }
-  req.m_internalBufs.append(internalBufs.begin(), internalBufs.end());
   req.m_userBufs.append(data.begin(), data.end());
 }
 

wpinet/src/main/native/cpp/uv/Async.cpp

@@ -17,6 +17,9 @@ Async<>::~Async() noexcept {
 }
 
 std::shared_ptr<Async<>> Async<>::Create(const std::shared_ptr<Loop>& loop) {
+  if (loop->IsClosing()) {
+    return nullptr;
+  }
   auto h = std::make_shared<Async>(loop, private_init{});
   int err = uv_async_init(loop->GetRaw(), h->GetRaw(), [](uv_async_t* handle) {
     Async& h = *static_cast<Async*>(handle->data);

wpinet/src/main/native/cpp/uv/Check.cpp

@@ -9,6 +9,9 @@
 namespace wpi::uv {
 
 std::shared_ptr<Check> Check::Create(Loop& loop) {
+  if (loop.IsClosing()) {
+    return nullptr;
+  }
   auto h = std::make_shared<Check>(private_init{});
   int err = uv_check_init(loop.GetRaw(), h->GetRaw());
   if (err < 0) {
@@ -20,6 +23,9 @@ std::shared_ptr<Check> Check::Create(Loop& loop) {
 }
 
 void Check::Start() {
+  if (IsLoopClosing()) {
+    return;
+  }
   Invoke(&uv_check_start, GetRaw(), [](uv_check_t* handle) {
     Check& h = *static_cast<Check*>(handle->data);
     h.check();

wpinet/src/main/native/cpp/uv/FsEvent.cpp

@@ -13,6 +13,9 @@
 namespace wpi::uv {
 
 std::shared_ptr<FsEvent> FsEvent::Create(Loop& loop) {
+  if (loop.IsClosing()) {
+    return nullptr;
+  }
   auto h = std::make_shared<FsEvent>(private_init{});
   int err = uv_fs_event_init(loop.GetRaw(), h->GetRaw());
   if (err < 0) {