Merge branch 'main' into development

.github/workflows/cmake.yml

@@ -88,28 +88,13 @@ jobs:
         uses: lukka/run-vcpkg@v11.1
         with:
           vcpkgDirectory: ${{ runner.workspace }}/vcpkg
-          vcpkgGitCommitId: 78b61582c9e093fda56a01ebb654be15a0033897 # HEAD on 2023-08-6
+          vcpkgGitCommitId: 37c3e63a1306562f7f59c4c3c8892ddd50fdf992 # HEAD on 2024-02-24
 
       - name: configure
-        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=ON -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
+        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=OFF -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
         env:
           SCCACHE_GHA_ENABLED: "true"
 
-      # Build wpiutil at full speed, wpimath depends on wpiutil
-      - name: build wpiutil
-        working-directory: build
-        run: cmake --build . --parallel $(nproc) --target wpiutil/all
-        env:
-          SCCACHE_GHA_ENABLED: "true"
-
-      # Build wpimath slow to prevent OOM
-      - name: build wpimath
-        working-directory: build
-        run: cmake --build . --parallel 1 --target wpimath/all
-        env:
-          SCCACHE_GHA_ENABLED: "true"
-
-      # Build everything else fast
       - name: build
         working-directory: build
         run: cmake --build . --parallel $(nproc)

.github/workflows/lint-format.yml

@@ -27,7 +27,7 @@ jobs:
         with:
           python-version: '3.10'
       - name: Install wpiformat
-        run: pip3 install wpiformat==2023.36
+        run: pip3 install wpiformat==2024.32
       - name: Run
         run: wpiformat
       - name: Check output

apriltag/convert_apriltag_layouts.py

@@ -82,6 +82,7 @@ def main():
         # Write JSON
         with open(filename.replace(".csv", ".json"), "w") as f:
             json.dump(json_data, f, indent=2)
+            f.write("\n")
 
 
 if __name__ == "__main__":

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java

@@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Translation3d;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.InputStreamReader;
+import java.io.UncheckedIOException;
 import java.nio.file.Path;
 import java.util.ArrayList;
 import java.util.HashMap;
@@ -221,6 +222,22 @@ public class AprilTagFieldLayout {
     new ObjectMapper().writeValue(path.toFile(), this);
   }
 
+  /**
+   * Get an official {@link AprilTagFieldLayout}.
+   *
+   * @param field The loadable AprilTag field layout.
+   * @return AprilTagFieldLayout of the field.
+   * @throws UncheckedIOException If the layout does not exist.
+   */
+  public static AprilTagFieldLayout loadField(AprilTagFields field) {
+    try {
+      return loadFromResource(field.m_resourceFile);
+    } catch (IOException e) {
+      throw new UncheckedIOException(
+          "Could not load AprilTagFieldLayout from " + field.m_resourceFile, e);
+    }
+  }
+
   /**
    * Deserializes a field layout from a resource within a internal jar file.
    *

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.apriltag;
 
-import java.io.IOException;
 import java.io.UncheckedIOException;
 
 /** Loadable AprilTag field layouts. */
@@ -36,11 +35,6 @@ public enum AprilTagFields {
    * @throws UncheckedIOException If the layout does not exist
    */
   public AprilTagFieldLayout loadAprilTagLayoutField() {
-    try {
-      return AprilTagFieldLayout.loadFromResource(m_resourceFile);
-    } catch (IOException e) {
-      throw new UncheckedIOException(
-          "Could not load AprilTagFieldLayout from " + m_resourceFile, e);
-    }
+    return AprilTagFieldLayout.loadField(this);
   }
 }

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -125,3 +125,37 @@ void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
   layout.m_fieldWidth =
       units::meter_t{json.at("field").at("width").get<double>()};
 }
+
+// Use namespace declaration for forward declaration
+namespace frc {
+
+// C++ generated from resource files
+std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2024_crescendo_json();
+
+}  // namespace frc
+
+AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
+  std::string_view fieldString;
+  switch (field) {
+    case AprilTagField::k2022RapidReact:
+      fieldString = GetResource_2022_rapidreact_json();
+      break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
+    case AprilTagField::k2024Crescendo:
+      fieldString = GetResource_2024_crescendo_json();
+      break;
+    case AprilTagField::kNumFields:
+      throw std::invalid_argument("Invalid Field");
+  }
+
+  wpi::json json = wpi::json::parse(fieldString);
+  return json.get<AprilTagFieldLayout>();
+}
+
+AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) {
+  return AprilTagFieldLayout::LoadField(field);
+}

apriltag/src/main/native/cpp/AprilTagFields.cpp

@@ -1,36 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include "frc/apriltag/AprilTagFields.h"
-
-#include <wpi/json.h>
-
-namespace frc {
-
-// C++ generated from resource files
-std::string_view GetResource_2022_rapidreact_json();
-std::string_view GetResource_2023_chargedup_json();
-std::string_view GetResource_2024_crescendo_json();
-
-AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
-  std::string_view fieldString;
-  switch (field) {
-    case AprilTagField::k2022RapidReact:
-      fieldString = GetResource_2022_rapidreact_json();
-      break;
-    case AprilTagField::k2023ChargedUp:
-      fieldString = GetResource_2023_chargedup_json();
-      break;
-    case AprilTagField::k2024Crescendo:
-      fieldString = GetResource_2024_crescendo_json();
-      break;
-    case AprilTagField::kNumFields:
-      throw std::invalid_argument("Invalid Field");
-  }
-
-  wpi::json json = wpi::json::parse(fieldString);
-  return json.get<AprilTagFieldLayout>();
-}
-
-}  // namespace frc

apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h

@@ -14,6 +14,7 @@
 #include <wpi/json_fwd.h>
 
 #include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFields.h"
 #include "frc/geometry/Pose3d.h"
 
 namespace frc {
@@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
     kRedAllianceWallRightSide,
   };
 
+  /**
+   * Loads an AprilTagFieldLayout from a predefined field
+   *
+   * @param field The predefined field
+   * @return AprilTagFieldLayout of the field
+   */
+  static AprilTagFieldLayout LoadField(AprilTagField field);
+
   AprilTagFieldLayout() = default;
 
   /**
@@ -152,4 +161,13 @@ void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
 WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
 
+/**
+ * Loads an AprilTagFieldLayout from a predefined field
+ *
+ * @param field The predefined field
+ * @return AprilTagFieldLayout of the field
+ */
+WPILIB_DLLEXPORT AprilTagFieldLayout
+LoadAprilTagLayoutField(AprilTagField field);
+
 }  // namespace frc

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -8,8 +8,6 @@
 
 #include <wpi/SymbolExports.h>
 
-#include "frc/apriltag/AprilTagFieldLayout.h"
-
 namespace frc {
 
 /**
@@ -28,12 +26,4 @@ enum class AprilTagField {
   kNumFields,
 };
 
-/**
- * Loads an AprilTagFieldLayout from a predefined field
- *
- * @param field The predefined field
- */
-WPILIB_DLLEXPORT AprilTagFieldLayout
-LoadAprilTagLayoutField(AprilTagField field);
-
 }  // namespace frc

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json

@@ -290,7 +290,7 @@
     }
   ],
   "field": {
-    "length": 16.451,
+    "length": 16.541,
     "width": 8.211
   }
 }

apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java

@@ -22,13 +22,14 @@ class LoadConfigTest {
   @ParameterizedTest
   @EnumSource(AprilTagFields.class)
   void testLoad(AprilTagFields field) {
-    AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField);
+    AprilTagFieldLayout layout =
+        Assertions.assertDoesNotThrow(() -> AprilTagFieldLayout.loadField(field));
     assertNotNull(layout);
   }
 
   @Test
   void test2022RapidReact() {
-    AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField();
+    AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact);
 
     // Blue Hangar Truss - Hub
     Pose3d expectedPose =

apriltag/src/test/native/cpp/LoadConfigTest.cpp

@@ -4,6 +4,7 @@
 
 #include <gtest/gtest.h>
 
+#include "frc/apriltag/AprilTagFieldLayout.h"
 #include "frc/apriltag/AprilTagFields.h"
 
 namespace frc {
@@ -20,7 +21,7 @@ std::vector<AprilTagField> GetAllFields() {
 
 TEST(AprilTagFieldsTest, TestLoad2022RapidReact) {
   AprilTagFieldLayout layout =
-      LoadAprilTagLayoutField(AprilTagField::k2022RapidReact);
+      AprilTagFieldLayout::LoadField(AprilTagField::k2022RapidReact);
 
   // Blue Hangar Truss - Hub
   auto expectedPose =
@@ -53,7 +54,7 @@ class AllFieldsFixtureTest : public ::testing::TestWithParam<AprilTagField> {};
 
 TEST_P(AllFieldsFixtureTest, CheckEntireEnum) {
   AprilTagField field = GetParam();
-  EXPECT_NO_THROW(LoadAprilTagLayoutField(field));
+  EXPECT_NO_THROW(AprilTagFieldLayout::LoadField(field));
 }
 
 INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest,

glass/src/lib/native/cpp/other/FMS.cpp

@@ -5,6 +5,7 @@
 #include "glass/other/FMS.h"
 
 #include <imgui.h>
+#include <imgui_stdlib.h>
 #include <wpi/SmallString.h>
 
 #include "glass/DataSource.h"
@@ -58,8 +59,7 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
   if (auto data = model->GetMatchTimeData()) {
     double val = data->GetValue();
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-    if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f",
-                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+    if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f")) {
       model->SetMatchTime(val);
     }
     data->EmitDrag();
@@ -78,16 +78,12 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
   }
 
   // Game Specific Message
-  // make buffer full 64 width, null terminated, for editability
-  wpi::SmallString<64> gameSpecificMessage;
-  model->GetGameSpecificMessage(gameSpecificMessage);
-  gameSpecificMessage.resize(63);
-  gameSpecificMessage.push_back('\0');
+  wpi::SmallString<64> gameSpecificMessageBuf;
+  std::string gameSpecificMessage{
+      model->GetGameSpecificMessage(gameSpecificMessageBuf)};
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-  if (ImGui::InputText("Game Specific", gameSpecificMessage.data(),
-                       gameSpecificMessage.size(),
-                       ImGuiInputTextFlags_EnterReturnsTrue)) {
-    model->SetGameSpecificMessage(gameSpecificMessage.data());
+  if (ImGui::InputText("Game Specific", &gameSpecificMessage)) {
+    model->SetGameSpecificMessage(gameSpecificMessage);
   }
 }
 
@@ -151,9 +147,10 @@ void glass::DisplayFMSReadOnly(FMSModel* model) {
     }
   }
 
-  wpi::SmallString<64> gameSpecificMessage;
-  model->GetGameSpecificMessage(gameSpecificMessage);
-  ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.c_str() : "?");
+  wpi::SmallString<64> gameSpecificMessageBuf;
+  std::string_view gameSpecificMessage =
+      model->GetGameSpecificMessage(gameSpecificMessageBuf);
+  ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.data() : "?");
 
   if (!exists) {
     ImGui::PopStyleColor();

hal/src/main/java/edu/wpi/first/hal/HAL.java

@@ -75,6 +75,9 @@ public final class HAL extends JNIWrapper {
    */
   public static native void exitMain();
 
+  /** Terminates the executable (at the native level). Does nothing in simulation. */
+  public static native void terminate();
+
   private static native void simPeriodicBeforeNative();
 
   private static final List<Runnable> s_simPeriodicBefore = new ArrayList<>();

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

@@ -539,15 +539,25 @@ HAL_Bool HAL_RefreshDSData(void) {
     }
     // If newest state shows we have a DS attached, just use the
     // control word out of the cache, As it will be the one in sync
-    // with the data. Otherwise use the state that shows disconnected.
-    if (controlWord.dsAttached) {
-      newestControlWord = currentRead->controlWord;
-    } else {
-      // Zero out the control word. When the DS has never been connected
-      // this returns garbage. And there is no way we can detect that.
-      std::memset(&controlWord, 0, sizeof(controlWord));
-      newestControlWord = controlWord;
+    // with the data. If no data has been updated, at this point,
+    // and a DS wasn't attached previously, this will still return
+    // a zeroed out control word, with is the correct state for
+    // no new data.
+    if (!controlWord.dsAttached) {
+      // If the DS is not attached, we need to zero out the control word.
+      // This is because HAL_RefreshDSData is called asynchronously from
+      // the DS data. The dsAttached variable comes directly from netcomm
+      // and could be updated before the caches are. If that happens,
+      // we would end up returning the previous cached control word,
+      // which is out of sync with the current control word and could
+      // break invariants such as which alliance station is in used.
+      // Also, when the DS has never been connected the rest of the fields
+      // in control word are garbage, so we also need to zero out in that
+      // case too
+      std::memset(&currentRead->controlWord, 0,
+                  sizeof(currentRead->controlWord));
     }
+    newestControlWord = currentRead->controlWord;
   }
 
   uint32_t mask = tcpMask.exchange(0);

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

@@ -524,7 +524,7 @@ static bool killExistingProgram(int timeout, int mode) {
   return true;
 }
 
-static void SetupNowRio(void) {
+static bool SetupNowRio(void) {
   nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
       nLoadOut::getTargetClass();
 
@@ -534,13 +534,13 @@ static void SetupNowRio(void) {
   status = dladdr(reinterpret_cast<void*>(tHMB::create), &info);
   if (status == 0) {
     fmt::print(stderr, "Failed to call dladdr on chipobject {}\n", dlerror());
-    return;
+    return false;
   }
 
   void* chipObjectLibrary = dlopen(info.dli_fname, RTLD_LAZY);
   if (chipObjectLibrary == nullptr) {
     fmt::print(stderr, "Failed to call dlopen on chipobject {}\n", dlerror());
-    return;
+    return false;
   }
 
   std::unique_ptr<tHMB> hmb;
@@ -548,9 +548,9 @@ static void SetupNowRio(void) {
   if (hmb == nullptr) {
     fmt::print(stderr, "Failed to open HMB on chipobject {}\n", status);
     dlclose(chipObjectLibrary);
-    return;
+    return false;
   }
-  wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb));
+  return wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb));
 }
 
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
@@ -593,7 +593,17 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     setNewDataSem(nullptr);
   });
 
-  SetupNowRio();
+  if (!SetupNowRio()) {
+    fmt::print(stderr,
+               "Failed to run SetupNowRio(). This is a fatal error. The "
+               "process is being terminated.\n");
+    std::fflush(stderr);
+    // Attempt to force a segfault to get a better java log
+    *reinterpret_cast<int*>(0) = 0;
+    // If that fails, terminate
+    std::terminate();
+    return false;
+  }
 
   int32_t status = 0;
 

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

@@ -7,6 +7,7 @@
 #include <jni.h>
 
 #include <cassert>
+#include <cstdlib>
 #include <cstring>
 
 #include <fmt/format.h>
@@ -82,6 +83,20 @@ Java_edu_wpi_first_hal_HAL_exitMain
   HAL_ExitMain();
 }
 
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    terminate
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_terminate
+  (JNIEnv*, jclass)
+{
+#ifdef __FRC_ROBORIO__
+  std::abort();
+#endif
+}
+
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    simPeriodicBeforeNative

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

@@ -44,6 +44,7 @@ struct JoystickDataCache {
   HAL_JoystickButtons buttons[kJoystickPorts];
   HAL_AllianceStationID allianceStation;
   double matchTime;
+  HAL_ControlWord controlWord;
 };
 static_assert(std::is_standard_layout_v<JoystickDataCache>);
 // static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -65,6 +66,16 @@ void JoystickDataCache::Update() {
   }
   allianceStation = SimDriverStationData->allianceStationId;
   matchTime = SimDriverStationData->matchTime;
+
+  HAL_ControlWord tmpControlWord;
+  std::memset(&tmpControlWord, 0, sizeof(tmpControlWord));
+  tmpControlWord.enabled = SimDriverStationData->enabled;
+  tmpControlWord.autonomous = SimDriverStationData->autonomous;
+  tmpControlWord.test = SimDriverStationData->test;
+  tmpControlWord.eStop = SimDriverStationData->eStop;
+  tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached;
+  tmpControlWord.dsAttached = SimDriverStationData->dsAttached;
+  this->controlWord = tmpControlWord;
 }
 
 #define CHECK_JOYSTICK_NUMBER(stickNum)                  \
@@ -322,20 +333,32 @@ HAL_Bool HAL_RefreshDSData(void) {
   if (gShutdown) {
     return false;
   }
-  HAL_ControlWord controlWord;
-  std::memset(&controlWord, 0, sizeof(controlWord));
-  controlWord.enabled = SimDriverStationData->enabled;
-  controlWord.autonomous = SimDriverStationData->autonomous;
-  controlWord.test = SimDriverStationData->test;
-  controlWord.eStop = SimDriverStationData->eStop;
-  controlWord.fmsAttached = SimDriverStationData->fmsAttached;
-  controlWord.dsAttached = SimDriverStationData->dsAttached;
+  bool dsAttached = SimDriverStationData->dsAttached;
   std::scoped_lock lock{driverStation->cacheMutex};
   JoystickDataCache* prev = currentCache.exchange(nullptr);
   if (prev != nullptr) {
     currentRead = prev;
   }
-  newestControlWord = controlWord;
+  // If newest state shows we have a DS attached, just use the
+  // control word out of the cache, As it will be the one in sync
+  // with the data. If no data has been updated, at this point,
+  // and a DS wasn't attached previously, this will still return
+  // a zeroed out control word, with is the correct state for
+  // no new data.
+  if (!dsAttached) {
+    // If the DS is not attached, we need to zero out the control word.
+    // This is because HAL_RefreshDSData is called asynchronously from
+    // the DS data. The dsAttached variable comes directly from netcomm
+    // and could be updated before the caches are. If that happens,
+    // we would end up returning the previous cached control word,
+    // which is out of sync with the current control word and could
+    // break invariants such as which alliance station is in used.
+    // Also, when the DS has never been connected the rest of the fields
+    // in control word are garbage, so we also need to zero out in that
+    // case too
+    std::memset(&currentRead->controlWord, 0, sizeof(currentRead->controlWord));
+  }
+  newestControlWord = currentRead->controlWord;
   return prev != nullptr;
 }
 
@@ -369,6 +392,7 @@ void NewDriverStationData() {
   if (gShutdown) {
     return;
   }
+  SimDriverStationData->dsAttached = true;
   cacheToUpdate->Update();
 
   JoystickDataCache* given = cacheToUpdate;
@@ -382,7 +406,6 @@ void NewDriverStationData() {
   }
   lastGiven = given;
 
-  SimDriverStationData->dsAttached = true;
   driverStation->newDataEvents.Wakeup();
   SimDriverStationData->CallNewDataCallbacks();
 }

ntcore/src/main/java/edu/wpi/first/networktables/PubSubOption.java

@@ -15,7 +15,8 @@ public class PubSubOption {
     disableRemote,
     disableLocal,
     excludePublisher,
-    excludeSelf
+    excludeSelf,
+    hidden
   }
 
   PubSubOption(Kind kind, boolean value) {
@@ -149,6 +150,18 @@ public class PubSubOption {
     return new PubSubOption(Kind.excludeSelf, enabled);
   }
 
+  /**
+   * For subscriptions, don't share the existence of the subscription with the network. Note this
+   * means updates will not be received from the network unless another subscription overlaps with
+   * this one, and the subscription will not appear in metatopics.
+   *
+   * @param enabled True to enable, false to disable
+   * @return option
+   */
+  public static PubSubOption hidden(boolean enabled) {
+    return new PubSubOption(Kind.hidden, enabled);
+  }
+
   final Kind m_kind;
   final boolean m_bValue;
   final int m_iValue;

ntcore/src/main/java/edu/wpi/first/networktables/PubSubOptions.java

@@ -42,6 +42,9 @@ public class PubSubOptions {
         case excludeSelf:
           excludeSelf = option.m_bValue;
           break;
+        case hidden:
+          hidden = option.m_bValue;
+          break;
         default:
           break;
       }
@@ -58,7 +61,8 @@ public class PubSubOptions {
       boolean prefixMatch,
       boolean disableRemote,
       boolean disableLocal,
-      boolean excludeSelf) {
+      boolean excludeSelf,
+      boolean hidden) {
     this.pollStorage = pollStorage;
     this.periodic = periodic;
     this.excludePublisher = excludePublisher;
@@ -69,6 +73,7 @@ public class PubSubOptions {
     this.disableRemote = disableRemote;
     this.disableLocal = disableLocal;
     this.excludeSelf = excludeSelf;
+    this.hidden = hidden;
   }
 
   /** Default value of periodic. */
@@ -123,4 +128,11 @@ public class PubSubOptions {
 
   /** For entries, don't queue (for readQueue) value updates for the entry's internal publisher. */
   public boolean excludeSelf;
+
+  /**
+   * For subscriptions, don't share the existence of the subscription with the network. Note this
+   * means updates will not be received from the network unless another subscription overlaps with
+   * this one, and the subscription will not appear in metatopics.
+   */
+  public boolean hidden;
 }

ntcore/src/main/native/cpp/LocalStorage.cpp

@@ -611,7 +611,7 @@ LocalStorage::SubscriberData* LocalStorage::Impl::AddLocalSubscriber(
         "published as '{}')",
         topic->name, config.typeStr, topic->typeStr);
   }
-  if (m_network) {
+  if (m_network && !subscriber->config.hidden) {
     DEBUG4("-> NetworkSubscribe({})", topic->name);
     m_network->Subscribe(subscriber->handle, {{topic->name}}, config);
   }
@@ -640,7 +640,7 @@ LocalStorage::Impl::RemoveLocalSubscriber(NT_Subscriber subHandle) {
         listener.getSecond()->subscriber = nullptr;
       }
     }
-    if (m_network) {
+    if (m_network && !subscriber->config.hidden) {
       m_network->Unsubscribe(subscriber->handle);
     }
   }
@@ -676,7 +676,7 @@ LocalStorage::MultiSubscriberData* LocalStorage::Impl::AddMultiSubscriber(
       }
     }
   }
-  if (m_network) {
+  if (m_network && !subscriber->options.hidden) {
     DEBUG4("-> NetworkSubscribe");
     m_network->Subscribe(subscriber->handle, subscriber->prefixes,
                          subscriber->options);
@@ -696,7 +696,7 @@ LocalStorage::Impl::RemoveMultiSubscriber(NT_MultiSubscriber subHandle) {
         listener.getSecond()->multiSubscriber = nullptr;
       }
     }
-    if (m_network) {
+    if (m_network && !subscriber->options.hidden) {
       m_network->Unsubscribe(subscriber->handle);
     }
   }
@@ -1128,12 +1128,16 @@ void LocalStorage::Impl::StartNetwork(net::NetworkInterface* network) {
     }
   }
   for (auto&& subscriber : m_subscribers) {
-    network->Subscribe(subscriber->handle, {{subscriber->topic->name}},
-                       subscriber->config);
+    if (!subscriber->config.hidden) {
+      network->Subscribe(subscriber->handle, {{subscriber->topic->name}},
+                         subscriber->config);
+    }
   }
   for (auto&& subscriber : m_multiSubscribers) {
-    network->Subscribe(subscriber->handle, subscriber->prefixes,
-                       subscriber->options);
+    if (!subscriber->options.hidden) {
+      network->Subscribe(subscriber->handle, subscriber->prefixes,
+                         subscriber->options);
+    }
   }
 }
 

ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp

@@ -139,6 +139,7 @@ static nt::PubSubOptions FromJavaPubSubOptions(JNIEnv* env, jobject joptions) {
   FIELD(disableRemote, "Z");
   FIELD(disableLocal, "Z");
   FIELD(excludeSelf, "Z");
+  FIELD(hidden, "Z");
 
 #undef FIELD
 
@@ -154,7 +155,8 @@ static nt::PubSubOptions FromJavaPubSubOptions(JNIEnv* env, jobject joptions) {
           FIELD(bool, Boolean, prefixMatch),
           FIELD(bool, Boolean, disableRemote),
           FIELD(bool, Boolean, disableLocal),
-          FIELD(bool, Boolean, excludeSelf)};
+          FIELD(bool, Boolean, excludeSelf),
+          FIELD(bool, Boolean, hidden)};
 
 #undef GET
 #undef FIELD

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

@@ -1816,7 +1816,8 @@ void ServerImpl::SetValue(ClientData* client, TopicData* topic,
   }
 
   for (auto&& tcd : topic->clients) {
-    if (tcd.second.sendMode != ValueSendMode::kDisabled) {
+    if (tcd.first != client &&
+        tcd.second.sendMode != ValueSendMode::kDisabled) {
       tcd.first->SendValue(topic, value, tcd.second.sendMode);
     }
   }

ntcore/src/main/native/cpp/ntcore_c.cpp

@@ -126,6 +126,7 @@ static PubSubOptions ConvertToCpp(const NT_PubSubOptions* in) {
   out.disableRemote = in->disableRemote;
   out.disableLocal = in->disableLocal;
   out.excludeSelf = in->excludeSelf;
+  out.hidden = in->hidden;
   return out;
 }
 

ntcore/src/main/native/include/ntcore_c.h

@@ -367,6 +367,14 @@ struct NT_PubSubOptions {
    * internal publisher.
    */
   NT_Bool excludeSelf;
+
+  /**
+   * For subscriptions, don't share the existence of the subscription with the
+   * network. Note this means updates will not be received from the network
+   * unless another subscription overlaps with this one, and the subscription
+   * will not appear in metatopics.
+   */
+  NT_Bool hidden;
 };
 
 /**

ntcore/src/main/native/include/ntcore_cpp.h

@@ -374,6 +374,14 @@ struct PubSubOptions {
    * internal publisher.
    */
   bool excludeSelf = false;
+
+  /**
+   * For subscriptions, don't share the existence of the subscription with the
+   * network. Note this means updates will not be received from the network
+   * unless another subscription overlaps with this one, and the subscription
+   * will not appear in metatopics.
+   */
+  bool hidden = false;
 };
 
 /**

sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp

@@ -4,6 +4,8 @@
 
 #include "sysid/analysis/FeedbackAnalysis.h"
 
+#include <cmath>
+
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
 #include <frc/system/plant/LinearSystemId.h>
@@ -21,6 +23,10 @@ using Ka_t = decltype(1_V / 1_mps_sq);
 FeedbackGains sysid::CalculatePositionFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
   // If acceleration requires no effort, velocity becomes an input for position
   // control. We choose an appropriate model in this case to avoid numerical
   // instabilities in the LQR.
@@ -32,7 +38,8 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
     frc::LinearQuadraticRegulator<2, 1> controller{
         system, {params.qp, params.qv}, {params.r}, preset.period};
     // Compensate for any latency from sensor measurements, filtering, etc.
-    controller.LatencyCompensate(system, preset.period, 0.0_s);
+    controller.LatencyCompensate(system, preset.period,
+                                 preset.measurementDelay);
 
     return {controller.K(0, 0) * preset.outputConversionFactor,
             controller.K(0, 1) * preset.outputConversionFactor /
@@ -47,7 +54,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qp}, {params.r}, preset.period};
   // Compensate for any latency from sensor measurements, filtering, etc.
-  controller.LatencyCompensate(system, preset.period, 0.0_s);
+  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
   return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
 }
@@ -55,6 +62,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
 FeedbackGains sysid::CalculateVelocityFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka, double encFactor) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
   // If acceleration for velocity control requires no effort, the feedback
   // control gains approach zero. We special-case it here because numerical
   // instabilities arise in LQR otherwise.

sysid/src/main/native/cpp/analysis/FilteringUtils.cpp

@@ -146,9 +146,18 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
                wpi::sgn(b.velocity) * b.acceleration;
       });
 
+  // Current limiting can delay onset of the peak acceleration, so we need to
+  // find the first acceleration *near* the max.  Magic number tolerance here
+  // because this whole file is tech debt already
+  auto accelBegins = std::find_if(
+      data->begin(), data->end(), [&maxAccel](const auto& measurement) {
+        return wpi::sgn(measurement.velocity) * measurement.acceleration >
+               0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration;
+      });
+
   units::second_t velocityDelay;
-  if (maxAccel != data->end()) {
-    velocityDelay = maxAccel->timestamp - firstTimestamp;
+  if (accelBegins != data->end()) {
+    velocityDelay = accelBegins->timestamp - firstTimestamp;
 
     // Trim data before max acceleration
     data->erase(data->begin(), maxAccel);

sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp

@@ -103,3 +103,30 @@ TEST(FeedbackAnalysisTest, VelocityREVConversion) {
   EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
+
+TEST(FeedbackAnalysisTest, Position) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 6.41, 0.05);
+  EXPECT_NEAR(Kd, 2.48, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+  sysid::FeedbackControllerPreset preset{sysid::presets::kDefault};
+
+  preset.measurementDelay = 10_ms;
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(preset, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 5.92, 0.05);
+  EXPECT_NEAR(Kd, 2.12, 0.05);
+}

vcpkg.json

@@ -3,10 +3,9 @@
   "version-string": "latest",
   "dependencies": [
     "opencv",
-    "eigen3",
     "fmt",
     "libuv",
     "protobuf"
   ],
-  "builtin-baseline": "78b61582c9e093fda56a01ebb654be15a0033897"
+  "builtin-baseline": "37c3e63a1306562f7f59c4c3c8892ddd50fdf992"
 }

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

@@ -63,7 +63,7 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
 
   @Override
   public void periodic() {
-    m_state = m_profile.calculate(m_period, m_goal, m_state);
+    m_state = m_profile.calculate(m_period, m_state, m_goal);
     if (m_enabled) {
       useState(m_state);
     }

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

@@ -45,7 +45,7 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
         m_period(period) {}
 
   void Periodic() override {
-    m_state = m_profile.Calculate(m_period, m_goal, m_state);
+    m_state = m_profile.Calculate(m_period, m_state, m_goal);
     if (m_enabled) {
       UseState(m_state);
     }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java

@@ -6,7 +6,6 @@ package edu.wpi.first.wpilibj2.command.button;
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj2.command.CommandTestBase;
 import org.junit.jupiter.api.Test;
@@ -18,7 +17,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(true);
     DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(true);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger auto = RobotModeTriggers.autonomous();
     assertTrue(auto.getAsBoolean());
   }
@@ -29,7 +28,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(true);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger teleop = RobotModeTriggers.teleop();
     assertTrue(teleop.getAsBoolean());
   }
@@ -40,7 +39,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(true);
     DriverStationSim.setEnabled(true);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger test = RobotModeTriggers.test();
     assertTrue(test.getAsBoolean());
   }
@@ -51,7 +50,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(false);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger disabled = RobotModeTriggers.disabled();
     assertTrue(disabled.getAsBoolean());
   }

wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp

@@ -18,7 +18,7 @@ TEST(RobotModeTriggersTest, Autonomous) {
   DriverStationSim::SetAutonomous(true);
   DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(true);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger autonomous = RobotModeTriggers::Autonomous();
   EXPECT_TRUE(autonomous.Get());
 }
@@ -28,7 +28,7 @@ TEST(RobotModeTriggersTest, Teleop) {
   DriverStationSim::SetAutonomous(false);
   DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(true);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger teleop = RobotModeTriggers::Teleop();
   EXPECT_TRUE(teleop.Get());
 }
@@ -38,7 +38,7 @@ TEST(RobotModeTriggersTest, Disabled) {
   DriverStationSim::SetAutonomous(false);
   DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(false);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger disabled = RobotModeTriggers::Disabled();
   EXPECT_TRUE(disabled.Get());
 }
@@ -48,7 +48,7 @@ TEST(RobotModeTriggersTest, TestMode) {
   DriverStationSim::SetAutonomous(false);
   DriverStationSim::SetTest(true);
   DriverStationSim::SetEnabled(true);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger test = RobotModeTriggers::Test();
   EXPECT_TRUE(test.Get());
 }

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

@@ -36,13 +36,12 @@ class SendableChooser : public SendableChooserBase {
   template <class U>
   static U _unwrap_smart_ptr(const U& value);
 
-  template <class U>
-  static U* _unwrap_smart_ptr(const std::unique_ptr<U>& value);
-
   template <class U>
   static std::weak_ptr<U> _unwrap_smart_ptr(const std::shared_ptr<U>& value);
 
  public:
+  using CopyType = decltype(_unwrap_smart_ptr(m_choices.lookup("")));
+
   SendableChooser() = default;
   ~SendableChooser() override = default;
   SendableChooser(SendableChooser&& rhs) = default;
@@ -71,8 +70,8 @@ class SendableChooser : public SendableChooserBase {
   void SetDefaultOption(std::string_view name, T object);
 
   /**
-   * Returns a copy of the selected option (a raw pointer U* if T =
-   * std::unique_ptr<U> or a std::weak_ptr<U> if T = std::shared_ptr<U>).
+   * Returns a copy of the selected option (a std::weak_ptr<U> if T =
+   * std::shared_ptr<U>).
    *
    * If there is none selected, it will return the default. If there is none
    * selected and no default, then it will return a value-initialized instance.
@@ -81,7 +80,7 @@ class SendableChooser : public SendableChooserBase {
    *
    * @return The option selected
    */
-  auto GetSelected() -> decltype(_unwrap_smart_ptr(m_choices[""]));
+  CopyType GetSelected() const;
 
   /**
    * Bind a listener that's called when the selected value changes.

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

@@ -33,8 +33,7 @@ void SendableChooser<T>::SetDefaultOption(std::string_view name, T object) {
 
 template <class T>
   requires std::copy_constructible<T> && std::default_initializable<T>
-auto SendableChooser<T>::GetSelected()
-    -> decltype(_unwrap_smart_ptr(m_choices[""])) {
+typename SendableChooser<T>::CopyType SendableChooser<T>::GetSelected() const {
   std::string selected = m_defaultChoice;
   {
     std::scoped_lock lock(m_mutex);
@@ -43,9 +42,9 @@ auto SendableChooser<T>::GetSelected()
     }
   }
   if (selected.empty()) {
-    return decltype(_unwrap_smart_ptr(m_choices[""])){};
+    return CopyType{};
   } else {
-    return _unwrap_smart_ptr(m_choices[selected]);
+    return _unwrap_smart_ptr(m_choices.lookup(selected));
   }
 }
 
@@ -121,13 +120,6 @@ U SendableChooser<T>::_unwrap_smart_ptr(const U& value) {
   return value;
 }
 
-template <class T>
-  requires std::copy_constructible<T> && std::default_initializable<T>
-template <class U>
-U* SendableChooser<T>::_unwrap_smart_ptr(const std::unique_ptr<U>& value) {
-  return value.get();
-}
-
 template <class T>
   requires std::copy_constructible<T> && std::default_initializable<T>
 template <class U>

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

@@ -38,7 +38,7 @@ class SendableChooserBase : public wpi::Sendable,
   std::string m_defaultChoice;
   std::string m_selected;
   bool m_haveSelected = false;
-  wpi::mutex m_mutex;
+  mutable wpi::mutex m_mutex;
   int m_instance;
   std::string m_previousVal;
   static std::atomic_int s_instances;

wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h

@@ -194,7 +194,7 @@ class SysIdRoutineLog {
  private:
   LogEntries m_logEntries;
   std::string m_logName;
-  bool m_stateInitialized;
+  bool m_stateInitialized = false;
   wpi::log::StringLogEntry m_state;
 };
 }  // namespace frc::sysid

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h

@@ -131,7 +131,7 @@ class Drivetrain {
   nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
 
   frc::AprilTagFieldLayout m_aprilTagFieldLayout{
-      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)};
+      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
   frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
 
   frc::PWMSparkMax m_leftLeader{1};

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

@@ -44,7 +44,6 @@ class Elevator : public frc2::PIDSubsystem {
 
  private:
   frc::PWMSparkMax m_motor{5};
-  double m_setpoint = 0;
 
 // Conversion value of potentiometer varies between the real world and
 // simulation

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h

@@ -41,7 +41,6 @@ class Wrist : public frc2::PIDSubsystem {
 
  private:
   frc::PWMSparkMax m_motor{6};
-  double m_setpoint = 0;
 
 // Conversion value of potentiometer varies between the real world and
 // simulation

wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp

@@ -15,14 +15,28 @@ void SysIdRoutineBot::ConfigureBindings() {
       [this] { return -m_driverController.GetLeftY(); },
       [this] { return -m_driverController.GetRightX(); }));
 
-  m_driverController.A().WhileTrue(
-      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
-  m_driverController.B().WhileTrue(
-      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
-  m_driverController.X().WhileTrue(
-      m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
-  m_driverController.Y().WhileTrue(
-      m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+  // Using bumpers as a modifier and combining it with the buttons so that we
+  // can have both sets of bindings at once
+  (m_driverController.A() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  (m_driverController.B() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  (m_driverController.X() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+  (m_driverController.Y() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+
+  m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand(
+      [this] { return m_driverController.GetLeftTriggerAxis(); }));
+
+  (m_driverController.A() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  (m_driverController.B() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  (m_driverController.X() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kForward));
+  (m_driverController.Y() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kReverse));
 }
 
 frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() {

wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp

@@ -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.
+
+#include "subsystems/Shooter.h"
+
+#include <frc2/command/Commands.h>
+#include <units/angle.h>
+#include <units/voltage.h>
+
+Shooter::Shooter() {
+  m_shooterEncoder.SetDistancePerPulse(
+      constants::shooter::kEncoderDistancePerPulse.value());
+}
+
+frc2::CommandPtr Shooter::RunShooterCommand(
+    std::function<double()> shooterSpeed) {
+  return frc2::cmd::Run(
+             [this, shooterSpeed] {
+               m_shooterMotor.SetVoltage(
+                   units::volt_t{m_shooterFeedback.Calculate(
+                       m_shooterEncoder.GetRate(), shooterSpeed())} +
+                   m_shooterFeedforward.Calculate(
+                       units::turns_per_second_t{shooterSpeed()}));
+               m_feederMotor.Set(constants::shooter::kFeederSpeed);
+             },
+             {this})
+      .WithName("Set Shooter Speed");
+}
+
+frc2::CommandPtr Shooter::SysIdQuasistatic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Quasistatic(direction);
+}
+
+frc2::CommandPtr Shooter::SysIdDynamic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Dynamic(direction);
+}

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

@@ -8,6 +8,7 @@
 #include <numbers>
 
 #include <units/angle.h>
+#include <units/angular_acceleration.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
 #include <units/time.h>
@@ -38,8 +39,13 @@ using kv_unit =
                          units::inverse<units::turns>>;
 using kv_unit_t = units::unit_t<kv_unit>;
 
+using ka_unit =
+    units::compound_unit<units::volts,
+                         units::inverse<units::turns_per_second_squared>>;
+using ka_unit_t = units::unit_t<ka_unit>;
+
 inline constexpr std::array<int, 2> kEncoderPorts = {4, 5};
-inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kEncoderReversed = false;
 inline constexpr int kEncoderCpr = 1024;
 inline constexpr units::turn_t kEncoderDistancePerPulse =
     units::turn_t{1.0} / static_cast<double>(kEncoderCpr);
@@ -55,6 +61,7 @@ inline constexpr double kP = 1.0;
 
 inline constexpr units::volt_t kS = 0.05_V;
 inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / units::turn_t{1};
 
 inline constexpr double kFeederSpeed = 0.5;
 }  // namespace shooter

wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h

@@ -9,6 +9,7 @@
 
 #include "Constants.h"
 #include "subsystems/Drive.h"
+#include "subsystems/Shooter.h"
 
 class SysIdRoutineBot {
  public:
@@ -21,4 +22,5 @@ class SysIdRoutineBot {
   frc2::CommandXboxController m_driverController{
       constants::oi::kDriverControllerPort};
   Drive m_drive;
+  Shooter m_shooter;
 };

wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h

@@ -0,0 +1,54 @@
+// 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 <functional>
+
+#include <frc/Encoder.h>
+#include <frc/RobotController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include "Constants.h"
+
+class Shooter : public frc2::SubsystemBase {
+ public:
+  Shooter();
+
+  frc2::CommandPtr RunShooterCommand(std::function<double()> shooterSpeed);
+  frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+  frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
+ private:
+  frc::PWMSparkMax m_shooterMotor{constants::shooter::kShooterMotorPort};
+  frc::PWMSparkMax m_feederMotor{constants::shooter::kFeederMotorPort};
+
+  frc::Encoder m_shooterEncoder{constants::shooter::kEncoderPorts[0],
+                                constants::shooter::kEncoderPorts[1],
+                                constants::shooter::kEncoderReversed};
+
+  frc2::sysid::SysIdRoutine m_sysIdRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
+                          std::nullopt},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            m_shooterMotor.SetVoltage(driveVoltage);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            log->Motor("shooter-wheel")
+                .voltage(m_shooterMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::turn_t{m_shooterEncoder.GetDistance()})
+                .velocity(
+                    units::turns_per_second_t{m_shooterEncoder.GetRate()});
+          },
+          this}};
+  frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0};
+  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+      constants::shooter::kS, constants::shooter::kV, constants::shooter::kA};
+};

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

@@ -453,6 +453,11 @@ public abstract class RobotBase implements AutoCloseable {
       runRobot(robotSupplier);
     }
 
+    // On RIO, this will just terminate rather than shutting down cleanly (it's a no-op in sim).
+    // It's not worth the risk of hanging on shutdown when we want the code to restart as quickly
+    // as possible.
+    HAL.terminate();
+
     HAL.shutdown();
 
     System.exit(0);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.ComputerVisionUtil;
 import edu.wpi.first.math.VecBuilder;
@@ -125,7 +126,8 @@ public class Drivetrain {
 
     m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal);
 
-    m_objectInField = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField().getTagPose(0).get();
+    m_objectInField =
+        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(0).get();
 
     SmartDashboard.putData("Field", m_fieldSim);
     SmartDashboard.putData("FieldEstimation", m_fieldApproximation);

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

@@ -57,6 +57,7 @@ public final class Constants {
     public static final double kVVoltSecondsPerRotation =
         // Should have value 12V at free speed...
         12.0 / kShooterFreeRPS;
+    public static final double kAVoltSecondsSquaredPerRotation = 0;
 
     public static final double kFeederSpeed = 0.5;
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java

@@ -4,9 +4,9 @@
 
 package edu.wpi.first.wpilibj.examples.sysid;
 
-import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
-
+import edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive;
+import edu.wpi.first.wpilibj.examples.sysid.subsystems.Shooter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 public class SysIdRoutineBot {
   // The robot's subsystems
   private final Drive m_drive = new Drive();
+  private final Shooter m_shooter = new Shooter();
 
   // The driver's controller
   CommandXboxController m_driverController =
@@ -42,10 +43,44 @@ public class SysIdRoutineBot {
 
     // Bind full set of SysId routine tests to buttons; a complete routine should run each of these
     // once.
-    m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    // Using bumpers as a modifier and combining it with the buttons so that we can have both sets
+    // of bindings at once
+    m_driverController
+        .a()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .b()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_driverController
+        .x()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .y()
+        .and(m_driverController.rightBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Control the shooter wheel with the left trigger
+    m_shooter.setDefaultCommand(m_shooter.runShooter(m_driverController::getLeftTriggerAxis));
+
+    m_driverController
+        .a()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .b()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_driverController
+        .x()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_driverController
+        .y()
+        .and(m_driverController.leftBumper())
+        .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java

@@ -122,10 +122,20 @@ public class Drive extends SubsystemBase {
         .withName("arcadeDrive");
   }
 
+  /**
+   * Returns a command that will execute a quasistatic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return m_sysIdRoutine.quasistatic(direction);
   }
 
+  /**
+   * Returns a command that will execute a dynamic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
   public Command sysIdDynamic(SysIdRoutine.Direction direction) {
     return m_sysIdRoutine.dynamic(direction);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java

@@ -0,0 +1,129 @@
+// 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.sysid.subsystems;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.sysid.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.function.DoubleSupplier;
+
+public class Shooter extends SubsystemBase {
+  // The motor on the shooter wheel .
+  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
+
+  // The motor on the feeder wheels.
+  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
+
+  // The shooter wheel encoder
+  private final Encoder m_shooterEncoder =
+      new Encoder(
+          ShooterConstants.kEncoderPorts[0],
+          ShooterConstants.kEncoderPorts[1],
+          ShooterConstants.kEncoderReversed);
+
+  // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+  private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+  // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+  private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
+  // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+  private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
+
+  // Create a new SysId routine for characterizing the shooter.
+  private final SysIdRoutine m_sysIdRoutine =
+      new SysIdRoutine(
+          // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              // Tell SysId how to plumb the driving voltage to the motor(s).
+              (Measure<Voltage> volts) -> {
+                m_shooterMotor.setVoltage(volts.in(Volts));
+              },
+              // Tell SysId how to record a frame of data for each motor on the mechanism being
+              // characterized.
+              log -> {
+                // Record a frame for the shooter motor.
+                log.motor("shooter-wheel")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_shooterMotor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .angularPosition(m_angle.mut_replace(m_shooterEncoder.getDistance(), Rotations))
+                    .angularVelocity(
+                        m_velocity.mut_replace(m_shooterEncoder.getRate(), RotationsPerSecond));
+              },
+              // Tell SysId to make generated commands require this subsystem, suffix test state in
+              // WPILog with this subsystem's name ("shooter")
+              this));
+  // PID controller to run the shooter wheel in closed-loop, set the constants equal to those
+  // calculated by SysId
+  private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0, 0);
+  // Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to
+  // those calculated by SysId
+  private final SimpleMotorFeedforward m_shooterFeedforward =
+      new SimpleMotorFeedforward(
+          ShooterConstants.kSVolts,
+          ShooterConstants.kVVoltSecondsPerRotation,
+          ShooterConstants.kAVoltSecondsSquaredPerRotation);
+
+  /** Creates a new Shooter subsystem. */
+  public Shooter() {
+    // Sets the distance per pulse for the encoders
+    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Returns a command that runs the shooter at a specifc velocity.
+   *
+   * @param shooterSpeed The commanded shooter wheel speed in rotations per second
+   */
+  public Command runShooter(DoubleSupplier shooterSpeed) {
+    // Run shooter wheel at the desired speed using a PID controller and feedforward.
+    return run(() -> {
+          m_shooterMotor.setVoltage(
+              m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble())
+                  + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble()));
+          m_feederMotor.set(ShooterConstants.kFeederSpeed);
+        })
+        .finallyDo(
+            () -> {
+              m_shooterMotor.stopMotor();
+              m_feederMotor.stopMotor();
+            })
+        .withName("runShooter");
+  }
+
+  /**
+   * Returns a command that will execute a quasistatic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  /**
+   * Returns a command that will execute a dynamic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
+  }
+}

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.math;
 
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 import org.ejml.simple.SimpleMatrix;
 
@@ -48,6 +49,16 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
     super(other);
   }
 
+  /**
+   * Returns an element of the vector at a specified row.
+   *
+   * @param row The row that the element is located at.
+   * @return An element of the vector.
+   */
+  public double get(int row) {
+    return get(row, 0);
+  }
+
   @Override
   public Vector<R> times(double value) {
     return new Vector<>(this.m_storage.scale(value));
@@ -105,12 +116,39 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
    * @return The norm.
    */
   public double norm() {
-    double squaredNorm = 0.0;
+    return Math.sqrt(dot(this));
+  }
 
-    for (int i = 0; i < getNumRows(); ++i) {
-      squaredNorm += get(i, 0) * get(i, 0);
-    }
+  /**
+   * Returns the unit vector parallel with this vector.
+   *
+   * @return The unit vector.
+   */
+  public Vector<R> unit() {
+    return div(norm());
+  }
 
-    return Math.sqrt(squaredNorm);
+  /**
+   * Returns the projection of this vector along another.
+   *
+   * @param other The vector to project along.
+   * @return The projection.
+   */
+  public Vector<R> projection(Vector<R> other) {
+    return other.times(dot(other)).div(other.dot(other));
+  }
+
+  /**
+   * Returns the cross product of 3 dimensional vectors a and b.
+   *
+   * @param a The vector to cross with b.
+   * @param b The vector to cross with a.
+   * @return The cross product of a and b.
+   */
+  public static Vector<N3> cross(Vector<N3> a, Vector<N3> b) {
+    return VecBuilder.fill(
+        a.get(1) * b.get(2) - a.get(2) * b.get(1),
+        a.get(2) * b.get(0) - a.get(0) * b.get(2),
+        a.get(0) * b.get(1) - a.get(1) * b.get(0));
   }
 }

wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java

@@ -52,7 +52,7 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
       throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
     }
     if (ka < 0.0) {
-      throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!");
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
     }
   }
 

wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java

@@ -55,7 +55,7 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
       throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
     }
     if (ka < 0.0) {
-      throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!");
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
     }
   }
 

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -37,7 +37,7 @@ public class SimpleMotorFeedforward {
       throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
     }
     if (ka < 0.0) {
-      throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!");
+      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
     }
   }
 

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

@@ -313,6 +313,15 @@ public class LinearFilter {
     return retVal;
   }
 
+  /**
+   * Returns the last value calculated by the LinearFilter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_outputs.getFirst();
+  }
+
   /**
    * Factorial of n.
    *

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

@@ -72,6 +72,15 @@ public class MedianFilter {
     }
   }
 
+  /**
+   * Returns the last value calculated by the MedianFilter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_valueBuffer.getFirst();
+  }
+
   /** Resets the filter, clearing the window of all elements. */
   public void reset() {
     m_orderedValues.clear();

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

@@ -64,6 +64,15 @@ public class SlewRateLimiter {
     return m_prevVal;
   }
 
+  /**
+   * Returns the value last calculated by the SlewRateLimiter.
+   *
+   * @return The last value.
+   */
+  public double lastValue() {
+    return m_prevVal;
+  }
+
   /**
    * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
    *

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

@@ -285,11 +285,12 @@ public class Quaternion implements ProtobufSerializable, StructSerializable {
    * @return The logarithm of this quaternion.
    */
   public Quaternion log() {
-    var scalar = Math.log(norm());
+    var norm = norm();
+    var scalar = Math.log(norm);
 
     var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
 
-    var s_norm = getW() / norm();
+    var s_norm = getW() / norm;
 
     if (Math.abs(s_norm + 1) < 1e-9) {
       return new Quaternion(scalar, -Math.PI, 0, 0);

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

@@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation2dProto;
 import edu.wpi.first.math.geometry.struct.Translation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -78,6 +81,16 @@ public class Translation2d
     this(x.in(Meters), y.in(Meters));
   }
 
+  /**
+   * Constructs a Translation2d from the provided translation vector's X and Y components. The
+   * values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  public Translation2d(Vector<N2> vector) {
+    this(vector.get(0), vector.get(1));
+  }
+
   /**
    * Calculates the distance between two translations in 2D space.
    *
@@ -110,6 +123,15 @@ public class Translation2d
     return m_y;
   }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  public Vector<N2> toVector() {
+    return VecBuilder.fill(m_x, m_y);
+  }
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

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

@@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation3dProto;
 import edu.wpi.first.math.geometry.struct.Translation3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -83,6 +86,16 @@ public class Translation3d
     this(x.in(Meters), y.in(Meters), z.in(Meters));
   }
 
+  /**
+   * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The
+   * values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  public Translation3d(Vector<N3> vector) {
+    this(vector.get(0), vector.get(1), vector.get(2));
+  }
+
   /**
    * Calculates the distance between two translations in 3D space.
    *
@@ -126,6 +139,15 @@ public class Translation3d
     return m_z;
   }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  public Vector<N3> toVector() {
+    return VecBuilder.fill(m_x, m_y, m_z);
+  }
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java

@@ -191,7 +191,7 @@ public class ExponentialProfile {
     var timing = calculateProfileTiming(current, inflectionPoint, goal, u);
 
     if (t < 0) {
-      return current;
+      return new State(current.position, current.velocity);
     } else if (t < timing.inflectionTime) {
       return new State(
           computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current));
@@ -200,7 +200,7 @@ public class ExponentialProfile {
           computeDistanceFromTime(t - timing.totalTime, -u, goal),
           computeVelocityFromTime(t - timing.totalTime, -u, goal));
     } else {
-      return goal;
+      return new State(goal.position, goal.velocity);
     }
   }
 

wpimath/src/main/native/cpp/controller/RamseteController.cpp

@@ -4,8 +4,6 @@
 
 #include "frc/controller/RamseteController.h"
 
-#include <cmath>
-
 #include "units/angle.h"
 #include "units/math.h"
 

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

@@ -36,15 +36,15 @@ Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
 }  // namespace
 
 Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+    : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
 
 Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
                Rotation3d rotation)
-    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+    : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
 
 Pose3d::Pose3d(const Pose2d& pose)
-    : m_translation(pose.X(), pose.Y(), 0_m),
-      m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
+    : m_translation{pose.X(), pose.Y(), 0_m},
+      m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
 
 Pose3d Pose3d::operator+(const Transform3d& other) const {
   return TransformBy(other);

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

@@ -136,11 +136,12 @@ Quaternion Quaternion::Log(const Quaternion& other) const {
 }
 
 Quaternion Quaternion::Log() const {
-  double scalar = std::log(Norm());
+  double norm = Norm();
+  double scalar = std::log(norm);
 
   double v_norm = m_v.norm();
 
-  double s_norm = W() / Norm();
+  double s_norm = W() / norm;
 
   if (std::abs(s_norm + 1) < 1e-9) {
     return Quaternion{scalar, -std::numbers::pi, 0, 0};

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

@@ -19,11 +19,11 @@ Transform3d::Transform3d(Pose3d initial, Pose3d final) {
 }
 
 Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+    : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
 
 Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
                          Rotation3d rotation)
-    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+    : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
 
 Transform3d Transform3d::Inverse() const {
   // We are rotating the difference between the translations

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

@@ -11,6 +11,9 @@
 
 using namespace frc;
 
+Translation2d::Translation2d(const Eigen::Vector2d& vector)
+    : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
+
 units::meter_t Translation2d::Distance(const Translation2d& other) const {
   return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
 }

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

@@ -19,6 +19,11 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
   m_z = rectangular.Z();
 }
 
+Translation3d::Translation3d(const Eigen::Vector3d& vector)
+    : m_x{units::meter_t{vector.x()}},
+      m_y{units::meter_t{vector.y()}},
+      m_z{units::meter_t{vector.z()}} {}
+
 units::meter_t Translation3d::Distance(const Translation3d& other) const {
   return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
                            units::math::pow<2>(other.m_y - m_y) +

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

@@ -371,6 +371,13 @@ class LinearFilter {
     return retVal;
   }
 
+  /**
+   * Returns the last value calculated by the LinearFilter.
+   *
+   * @return The last value.
+   */
+  T LastValue() const { return m_outputs.front(); }
+
  private:
   wpi::circular_buffer<T> m_inputs;
   wpi::circular_buffer<T> m_outputs;

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

@@ -61,6 +61,13 @@ class MedianFilter {
     }
   }
 
+  /**
+   * Returns the last value calculated by the MedianFilter.
+   *
+   * @return The last value.
+   */
+  T LastValue() const { return m_valueBuffer.front(); }
+
   /**
    * Resets the filter, clearing the window of all elements.
    */

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

@@ -75,6 +75,13 @@ class SlewRateLimiter {
     return m_prevVal;
   }
 
+  /**
+   * Returns the value last calculated by the SlewRateLimiter.
+   *
+   * @return The last value.
+   */
+  Unit_t LastValue() const { return m_prevVal; }
+
   /**
    * Resets the slew rate limiter to the specified value; ignores the rate limit
    * when doing so.

wpimath/src/main/native/include/frc/geometry/Pose2d.inc

@@ -13,11 +13,11 @@
 namespace frc {
 
 constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+    : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
 
 constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
                          Rotation2d rotation)
-    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+    : m_translation{x, y}, m_rotation{std::move(rotation)} {}
 
 constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
   return TransformBy(other);

wpimath/src/main/native/include/frc/geometry/Rotation2d.inc

@@ -12,9 +12,9 @@
 namespace frc {
 
 constexpr Rotation2d::Rotation2d(units::angle_unit auto value)
-    : m_value(value),
-      m_cos(gcem::cos(value.template convert<units::radian>().value())),
-      m_sin(gcem::sin(value.template convert<units::radian>().value())) {}
+    : m_value{value},
+      m_cos{gcem::cos(value.template convert<units::radian>().value())},
+      m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
 
 constexpr Rotation2d::Rotation2d(double x, double y) {
   double magnitude = gcem::hypot(x, y);
@@ -33,7 +33,7 @@ constexpr Rotation2d Rotation2d::operator-() const {
 }
 
 constexpr Rotation2d Rotation2d::operator*(double scalar) const {
-  return Rotation2d(m_value * scalar);
+  return Rotation2d{m_value * scalar};
 }
 
 constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {

wpimath/src/main/native/include/frc/geometry/Transform2d.inc

@@ -14,11 +14,11 @@ namespace frc {
 
 constexpr Transform2d::Transform2d(Translation2d translation,
                                    Rotation2d rotation)
-    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+    : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
 
 constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
                                    Rotation2d rotation)
-    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+    : m_translation{x, y}, m_rotation{std::move(rotation)} {}
 
 constexpr Transform2d Transform2d::Inverse() const {
   // We are rotating the difference between the translations

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

@@ -7,6 +7,7 @@
 #include <initializer_list>
 #include <span>
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
 
+  /**
+   * Constructs a Translation2d from the provided translation vector's X and Y
+   * components. The values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  explicit Translation2d(const Eigen::Vector2d& vector);
+
   /**
    * Calculates the distance between two translations in 2D space.
    *
@@ -73,6 +82,13 @@ class WPILIB_DLLEXPORT Translation2d {
    */
   constexpr units::meter_t Y() const { return m_y; }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  constexpr Eigen::Vector2d ToVector() const;
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

wpimath/src/main/native/include/frc/geometry/Translation2d.inc

@@ -10,11 +10,15 @@
 namespace frc {
 
 constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
-    : m_x(x), m_y(y) {}
+    : m_x{x}, m_y{y} {}
 
 constexpr Translation2d::Translation2d(units::meter_t distance,
                                        const Rotation2d& angle)
-    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+    : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
+
+constexpr Eigen::Vector2d Translation2d::ToVector() const {
+  return Eigen::Vector2d{{m_x.value(), m_y.value()}};
+}
 
 constexpr Rotation2d Translation2d::Angle() const {
   return Rotation2d{m_x.value(), m_y.value()};

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

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -47,6 +48,14 @@ class WPILIB_DLLEXPORT Translation3d {
    */
   Translation3d(units::meter_t distance, const Rotation3d& angle);
 
+  /**
+   * Constructs a Translation3d from the provided translation vector's X, Y, and
+   * Z components. The values are assumed to be in meters.
+   *
+   * @param vector The translation vector to represent.
+   */
+  explicit Translation3d(const Eigen::Vector3d& vector);
+
   /**
    * Calculates the distance between two translations in 3D space.
    *
@@ -80,6 +89,13 @@ class WPILIB_DLLEXPORT Translation3d {
    */
   constexpr units::meter_t Z() const { return m_z; }
 
+  /**
+   * Returns a vector representation of this translation.
+   *
+   * @return A Vector representation of this translation.
+   */
+  constexpr Eigen::Vector3d ToVector() const;
+
   /**
    * Returns the norm, or distance from the origin to the translation.
    *

wpimath/src/main/native/include/frc/geometry/Translation3d.inc

@@ -13,12 +13,16 @@ namespace frc {
 
 constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
                                        units::meter_t z)
-    : m_x(x), m_y(y), m_z(z) {}
+    : m_x{x}, m_y{y}, m_z{z} {}
 
 constexpr Translation2d Translation3d::ToTranslation2d() const {
   return Translation2d{m_x, m_y};
 }
 
+constexpr Eigen::Vector3d Translation3d::ToVector() const {
+  return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
+}
+
 constexpr Translation3d Translation3d::operator+(
     const Translation3d& other) const {
   return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};

wpimath/src/test/java/edu/wpi/first/math/VectorTest.java

@@ -65,4 +65,40 @@ class VectorTest {
     assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm());
     assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm());
   }
+
+  @Test
+  void testVectorUnit() {
+    assertEquals(VecBuilder.fill(3.0, 4.0).unit(), VecBuilder.fill(3.0 / 5.0, 4.0 / 5.0));
+    assertEquals(VecBuilder.fill(8.0, 15.0).unit(), VecBuilder.fill(8.0 / 17.0, 15.0 / 17.0));
+  }
+
+  @Test
+  void testVectorProjection() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var res1 = vec1.projection(vec2);
+
+    assertEquals(res1.get(0), 1.662, 0.01);
+    assertEquals(res1.get(1), 2.077, 0.01);
+    assertEquals(res1.get(2), 2.49, 0.01);
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var res2 = vec4.projection(vec3);
+
+    assertEquals(res2.get(0), 2.29, 0.01);
+    assertEquals(res2.get(1), -4.57, 0.01);
+    assertEquals(res2.get(2), 6.86, 0.01);
+  }
+
+  @Test
+  void testVectorCross() {
+    var e1 = VecBuilder.fill(1.0, 0.0, 0.0);
+    var e2 = VecBuilder.fill(0.0, 1.0, 0.0);
+    assertEquals(Vector.cross(e1, e2), VecBuilder.fill(0.0, 0.0, 1.0));
+
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(3.0, 4.0, 5.0);
+    assertEquals(Vector.cross(vec1, vec2), VecBuilder.fill(-2.0, 4.0, -2.0));
+  }
 }

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

@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import edu.wpi.first.math.VecBuilder;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -131,4 +132,15 @@ class Translation2dTest {
     assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
     assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
   }
+
+  @Test
+  void testToVector() {
+    var vec = VecBuilder.fill(1.0, 2.0);
+    var translation = new Translation2d(vec);
+
+    assertEquals(vec.get(0), translation.getX());
+    assertEquals(vec.get(1), translation.getY());
+
+    assertEquals(vec, translation.toVector());
+  }
 }

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

@@ -160,4 +160,16 @@ class Translation3dTest {
         () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
         () -> assertEquals(0.0, two.getZ(), kEpsilon));
   }
+
+  @Test
+  void testToVector() {
+    var vec = VecBuilder.fill(1.0, 2.0, 3.0);
+    var translation = new Translation3d(vec);
+
+    assertEquals(vec.get(0), translation.getX());
+    assertEquals(vec.get(1), translation.getY());
+    assertEquals(vec.get(2), translation.getZ());
+
+    assertEquals(vec, translation.toVector());
+  }
 }

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

@@ -126,6 +126,16 @@ TEST(Translation2dTest, Nearest) {
       translation2.Y().value());
 }
 
+TEST(Translation2dTest, ToVector) {
+  const Eigen::Vector2d vec(1.0, 2.0);
+  const Translation2d translation{vec};
+
+  EXPECT_DOUBLE_EQ(vec[0], translation.X().value());
+  EXPECT_DOUBLE_EQ(vec[1], translation.Y().value());
+
+  EXPECT_TRUE(vec == translation.ToVector());
+}
+
 TEST(Translation2dTest, Constexpr) {
   constexpr Translation2d defaultCtor;
   constexpr Translation2d componentCtor{1_m, 2_m};

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

@@ -128,6 +128,17 @@ TEST(Translation3dTest, PolarConstructor) {
   EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
 }
 
+TEST(Translation3dTest, ToVector) {
+  const Eigen::Vector3d vec(1.0, 2.0, 3.0);
+  const Translation3d translation{vec};
+
+  EXPECT_DOUBLE_EQ(vec[0], translation.X().value());
+  EXPECT_DOUBLE_EQ(vec[1], translation.Y().value());
+  EXPECT_DOUBLE_EQ(vec[2], translation.Z().value());
+
+  EXPECT_TRUE(vec == translation.ToVector());
+}
+
 TEST(Translation3dTest, Constexpr) {
   constexpr Translation3d defaultCtor;
   constexpr Translation3d componentCtor{1_m, 2_m, 3_m};

wpinet/build.gradle

@@ -206,7 +206,7 @@ cppHeadersZip {
     from('src/main/native/thirdparty/libuv/include') {
         into '/'
     }
-    from('src/main/native/third_party/tcpsockets/include') {
+    from('src/main/native/thirdparty/tcpsockets/include') {
         into '/'
     }
 }

wpiunits/src/main/java/edu/wpi/first/units/Distance.java

@@ -5,10 +5,10 @@
 package edu.wpi.first.units;
 
 /**
- * Unit of angular dimension.
+ * Unit of linear dimension.
  *
- * <p>This is the base type for units of distance dimension. It is also used to specify the
- * dimension for {@link Measure}: <code>Measure&lt;Distance&gt;</code>.
+ * <p>This is the base type for units of linear dimension. It is also used to specify the dimension
+ * for {@link Measure}: <code>Measure&lt;Distance&gt;</code>.
  *
  * <p>Actual units (such as {@link Units#Meters} and {@link Units#Inches}) can be found in the
  * {@link Units} class.

wpiutil/src/main/native/cpp/timestamp.cpp

@@ -133,13 +133,15 @@ struct HMBLowLevel {
                                                           dlcloseWrapper};
 };
 struct HMBHolder {
-  void Configure(void* col, std::unique_ptr<fpga::tHMB> hmbObject) {
+  bool Configure(void* col, std::unique_ptr<fpga::tHMB> hmbObject) {
     hmb = std::move(hmbObject);
     chipObjectLibrary.reset(col);
     if (!lowLevel.Configure(hmb->getSystemInterface()->getHandle())) {
       hmb = nullptr;
       chipObjectLibrary = nullptr;
+      return false;
     }
+    return true;
   }
   void Reset() {
     lowLevel.Reset();
@@ -236,20 +238,22 @@ void wpi::impl::SetupNowDefaultOnRio() {
 
 #ifdef __FRC_ROBORIO__
 template <>
-void wpi::impl::SetupNowRio(void* chipObjectLibrary,
+bool wpi::impl::SetupNowRio(void* chipObjectLibrary,
                             std::unique_ptr<fpga::tHMB> hmbObject) {
   if (!hmbInitialized.test()) {
-    hmb.Configure(chipObjectLibrary, std::move(hmbObject));
+    return hmb.Configure(chipObjectLibrary, std::move(hmbObject));
   }
+  return true;
 }
 #endif
 
-void wpi::impl::SetupNowRio(uint32_t session) {
+bool wpi::impl::SetupNowRio(uint32_t session) {
 #ifdef __FRC_ROBORIO__
   if (!hmbInitialized.test()) {
-    hmb.lowLevel.Configure(session);
+    return hmb.lowLevel.Configure(session);
   }
 #endif
+  return true;
 }
 
 void wpi::impl::ShutdownNowRio() {
@@ -269,10 +273,14 @@ uint64_t wpi::Now() {
     if (nowUseDefaultOnFailure.test()) {
       return timestamp() - offset_val;
     } else {
-      fmt::print(
-          stderr,
-          "FPGA not yet configured in wpi::Now(). Time will not be correct.\n");
+      fmt::print(stderr,
+                 "FPGA not yet configured in wpi::Now(). This is a fatal "
+                 "error. The process is being terminated.\n");
       std::fflush(stderr);
+      // Attempt to force a segfault to get a better java log
+      *reinterpret_cast<int*>(0) = 0;
+      // If that fails, terminate
+      std::terminate();
       return 1;
     }
   }
@@ -311,7 +319,7 @@ void WPI_Impl_SetupNowUseDefaultOnRio(void) {
 }
 
 void WPI_Impl_SetupNowRioWithSession(uint32_t session) {
-  return wpi::impl::SetupNowRio(session);
+  wpi::impl::SetupNowRio(session);
 }
 
 void WPI_Impl_ShutdownNowRio(void) {

wpiutil/src/main/native/include/wpi/timestamp.h

@@ -74,10 +74,12 @@ void SetupNowDefaultOnRio();
  */
 #ifdef __FRC_ROBORIO__
 template <typename T>
-void SetupNowRio(void* chipObjectLibrary, std::unique_ptr<T> hmbObject);
+bool SetupNowRio(void* chipObjectLibrary, std::unique_ptr<T> hmbObject);
 #else
 template <typename T>
-inline void SetupNowRio(void*, std::unique_ptr<T>) {}
+inline bool SetupNowRio(void*, std::unique_ptr<T>) {
+  return true;
+}
 #endif
 
 /**
@@ -85,7 +87,7 @@ inline void SetupNowRio(void*, std::unique_ptr<T>) {}
  * No effect on non-Rio platforms. This take an FPGA session that has
  * already been initialized, and is used from LabVIEW.
  */
-void SetupNowRio(uint32_t session);
+bool SetupNowRio(uint32_t session);
 
 /**
  * De-initialize the on-Rio Now() implementation. No effect on non-Rio