[wpilib] Shuffleboard: Keep duplicates on SelectTab() (#5198)

If f(x, u) has no dynamics, the truncation error can be zero.

LICENSE.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2022 FIRST and other WPILib contributors
+Copyright (c) 2009-2023 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

MavenArtifacts.md

@@ -16,7 +16,7 @@ We provide two base types of artifacts.
 
 The first types are Java artifacts. These are usually published as `jar` files. Usually, the actual jar file is published with no classifier. The sources are published with the `-sources` classifier, and the javadocs are published with the `-javadoc` classifier.
 
-The second types are native artifacts. These are usually published as `zip` files (except for the `JNI` artifact types, which are `jar` files. See below for information on this). The `-sources` and `-headers` classifiers contain the sources and headers respecively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The platform artifact only contains the binaries for a specific platform. In addition, we provide a `-all` classifier. This classifer combines all of the platform artifacts into a single artifact. This is useful for tools that cannot determine what version to use during builds. However, we recommend using the platform specific classifier when possible. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those.
+The second types are native artifacts. These are usually published as `zip` files (except for the `JNI` artifact types, which are `jar` files. See below for information on this). The `-sources` and `-headers` classifiers contain the sources and headers respectively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The platform artifact only contains the binaries for a specific platform. In addition, we provide a `-all` classifier. This classifier combines all of the platform artifacts into a single artifact. This is useful for tools that cannot determine what version to use during builds. However, we recommend using the platform specific classifier when possible. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those.
 
 ## Artifact Names
 

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

@@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
   /**
    * Sets the origin for tag pose transformation.
    *
-   * This tranforms the Pose3ds returned by GetTagPose(int) to return the
+   * This transforms the Pose3ds returned by GetTagPose(int) to return the
    * correct pose relative to the provided origin.
    *
    * @param origin The new origin for tag transformations

cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java

@@ -143,7 +143,7 @@ public class CameraServerJNI {
   public static native void releaseSource(int source);
 
   //
-  // Camera Source Common Property Fuctions
+  // Camera Source Common Property Functions
   //
   public static native void setCameraBrightness(int source, int brightness);
 

cscore/src/main/native/cpp/UnlimitedHandleResource.h

@@ -16,7 +16,7 @@
 namespace cs {
 
 // The UnlimitedHandleResource class is a way to track handles. This version
-// allows an unlimted number of handles that are allocated sequentially. When
+// allows an unlimited number of handles that are allocated sequentially. When
 // possible, indices are reused to save memory usage and keep the array length
 // down.
 // However, automatic array management has not been implemented, but might be in

cscore/src/main/native/cpp/cscore_cpp.cpp

@@ -439,7 +439,7 @@ void ReleaseSource(CS_Source source, CS_Status* status) {
 }
 
 //
-// Camera Source Common Property Fuctions
+// Camera Source Common Property Functions
 //
 
 void SetCameraBrightness(CS_Source source, int brightness, CS_Status* status) {

cscore/src/main/native/include/cscore_c.h

@@ -323,7 +323,7 @@ void CS_ReleaseSource(CS_Source source, CS_Status* status);
 /** @} */
 
 /**
- * @defgroup cscore_source_prop_cfunc Camera Source Common Property Fuctions
+ * @defgroup cscore_source_prop_cfunc Camera Source Common Property Functions
  * @{
  */
 void CS_SetCameraBrightness(CS_Source source, int brightness,

cscore/src/main/native/include/cscore_cpp.h

@@ -262,7 +262,7 @@ void ReleaseSource(CS_Source source, CS_Status* status);
 /** @} */
 
 /**
- * @defgroup cscore_camera_property_func Camera Source Common Property Fuctions
+ * @defgroup cscore_camera_property_func Camera Source Common Property Functions
  * @{
  */
 void SetCameraBrightness(CS_Source source, int brightness, CS_Status* status);

cscore/src/main/native/objcpp/UsbCameraImplObjc.mm

@@ -79,7 +79,7 @@ using namespace cs;
     default:
       OBJCERROR(
           "Camera access explicitly blocked for application. No cameras are "
-          "accessable");
+          "accessible");
       self.isAuthorized = false;
       // TODO log
       break;
@@ -524,7 +524,7 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
   if (!self.isAuthorized) {
     OBJCERROR(
         "Camera access not authorized for application. No cameras are "
-        "accessable");
+        "accessible");
     return false;
   }
 

cscore/src/main/native/windows/COMCreators.cpp

@@ -78,7 +78,7 @@ STDMETHODIMP SourceReaderCB::OnReadSample(HRESULT hrStatus, DWORD dwStreamIndex,
     return S_OK;
   if (SUCCEEDED(hrStatus)) {
     if (pSample) {
-      // Prcoess sample
+      // Process sample
       source->ProcessFrame(pSample, m_mode);
       // DO NOT release the frame
     }

cscore/src/test/java/edu/wpi/first/cscore/UsbCameraTest.java

@@ -24,7 +24,7 @@ class UsbCameraTest {
   static class ConnectVerbose {
     @Test
     void setConnectVerboseEnabledTest() {
-      try (UsbCamera camera = new UsbCamera("Nonexistant Camera", getNonexistentCameraDev())) {
+      try (UsbCamera camera = new UsbCamera("Nonexistent Camera", getNonexistentCameraDev())) {
         camera.setConnectVerbose(1);
 
         CompletableFuture<String> result = new CompletableFuture<>();
@@ -38,7 +38,7 @@ class UsbCameraTest {
 
     @Test
     void setConnectVerboseDisabledTest() {
-      try (UsbCamera camera = new UsbCamera("Nonexistant Camera", getNonexistentCameraDev())) {
+      try (UsbCamera camera = new UsbCamera("Nonexistent Camera", getNonexistentCameraDev())) {
         camera.setConnectVerbose(0);
 
         CompletableFuture<String> result = new CompletableFuture<>();

datalogtool/src/main/native/cpp/Exporter.cpp

@@ -466,7 +466,8 @@ static void ValueToCsv(wpi::raw_ostream& os, const Entry& entry,
       fmt::print(os, "{}", val);
       return;
     }
-  } else if (entry.type == "int64") {
+  } else if (entry.type == "int64" || entry.type == "int") {
+    // support "int" for compatibility with old NT4 datalogs
     int64_t val;
     if (record.GetInteger(&val)) {
       fmt::print(os, "{}", val);

docs/build.gradle

@@ -239,7 +239,7 @@ task generateJavaDocs(type: Javadoc) {
     if (JavaVersion.current().isJava8Compatible() && project.hasProperty('docWarningsAsErrors')) {
         // Treat javadoc warnings as errors.
         //
-        // The second argument '-quiet' is a hack. The one paramater
+        // The second argument '-quiet' is a hack. The one parameter
         // addStringOption() doesn't work, so we add '-quiet', which is added
         // anyway by gradle. See https://github.com/gradle/gradle/issues/2354.
         //

docs/theme.css

@@ -151,7 +151,7 @@ html {
     --side-nav-arrow-opacity: 0;
     --side-nav-arrow-hover-opacity: 0.9;
 
-    /* height of an item in any tree / collapsable table */
+    /* height of an item in any tree / collapsible table */
     --tree-item-height: 30px;
 
     --darkmode-toggle-button-icon: "☀️";
@@ -1615,7 +1615,7 @@ SOFTWARE.
 
 html {
     /* side nav width. MUST be = `TREEVIEW_WIDTH`.
-     * Make sure it is wide enought to contain the page title (logo + title + version)
+     * Make sure it is wide enough to contain the page title (logo + title + version)
      */
     --side-nav-fixed-width: 340px;
     --menu-display: none;

fieldImages/src/main/java/edu/wpi/fields/FieldConfig.java

@@ -79,7 +79,7 @@ public class FieldConfig {
    *
    * @param resourcePath The path to the resource file
    * @return The field configuration
-   * @throws IOException Throws if the resoure could not be loaded
+   * @throws IOException Throws if the resource could not be loaded
    */
   public static FieldConfig loadFromResource(String resourcePath) throws IOException {
     try (InputStream stream = FieldConfig.class.getResourceAsStream(resourcePath);

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

@@ -32,7 +32,7 @@ public class SimDevice implements AutoCloseable {
    * Creates a simulated device.
    *
    * <p>The device name must be unique. Returns null if the device name already exists. If multiple
-   * instances of the same device are desired, recommend appending the instance/unique identifer in
+   * instances of the same device are desired, recommend appending the instance/unique identifier in
    * brackets to the base name, e.g. "device[1]".
    *
    * <p>null is returned if not in simulation.

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

@@ -13,7 +13,7 @@ public class SimDeviceJNI extends JNIWrapper {
    * Creates a simulated device.
    *
    * <p>The device name must be unique. 0 is returned if the device name already exists. If multiple
-   * instances of the same device are desired, recommend appending the instance/unique identifer in
+   * instances of the same device are desired, recommend appending the instance/unique identifier in
    * brackets to the base name, e.g. "device[1]".
    *
    * <p>0 is returned if not in simulation.

hal/src/main/native/athena/AnalogInternal.h

@@ -69,7 +69,7 @@ void setAnalogNumChannelsToActivate(int32_t channels);
  * number of active channels and the sample rate.
  *
  * When the number of channels changes, use the new value.  Otherwise,
- * return the curent value.
+ * return the current value.
  *
  * @return Value to write to the active channels field.
  */

hal/src/main/native/athena/FPGAEncoder.h

@@ -37,7 +37,7 @@ int32_t HAL_GetFPGAEncoder(HAL_FPGAEncoderHandle fpgaEncoderHandle,
 /**
  * Returns the period of the most recent pulse.
  * Returns the period of the most recent Encoder pulse in seconds.
- * This method compenstates for the decoding type.
+ * This method compensates for the decoding type.
  *
  * @deprecated Use GetRate() in favor of this method.  This returns unscaled
  * periods and GetRate() scales using value from SetDistancePerPulse().

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

@@ -316,11 +316,8 @@ void InitializeRoboRioComments(void) {
       return;
     }
     start += searchString.size();
-    size_t end = fileContents.find("\"", start);
-    if (end == std::string_view::npos) {
-      end = fileContents.size();
-    }
-    std::string_view escapedComments = wpi::slice(fileContents, start, end);
+    std::string_view escapedComments =
+        wpi::slice(fileContents, start, fileContents.size());
     wpi::SmallString<64> buf;
     auto [unescapedComments, rem] = wpi::UnescapeCString(escapedComments, buf);
     unescapedComments.copy(roboRioCommentsString,

hal/src/main/native/athena/frccansae/CANDeviceInterface.h

@@ -51,7 +51,7 @@ struct DeviceDescriptor
    // Bootloader version.  Will not change for the life of the product, but additional
    // field upgrade features could be added in newer hardware.
    char bootloaderRev[MAX_STRING_LEN];
-   // Manufacture Date.  Could be a calender date or just the FRC season year.
+   // Manufacture Date.  Could be a calendar date or just the FRC season year.
    // Also helps troubleshooting "old ones" vs "new ones".
    char manufactureDate[MAX_STRING_LEN];
    // General status of the hardware.  For example if the device is in bootloader

hal/src/main/native/athena/rev/PHFrames.h

@@ -79,7 +79,7 @@ extern "C" {
 /**
  * Signals in message Compressor_Config.
  *
- * Configures compressor to use digitial/analog sensors
+ * Configures compressor to use digital/analog sensors
  *
  * All signal values are as on the CAN bus.
  */

hal/src/main/native/include/hal/DMA.h

@@ -40,7 +40,7 @@ extern "C" {
 #endif
 
 /**
- * Initializes an object for peforming DMA transfers.
+ * Initializes an object for performing DMA transfers.
  *
  * @param[out] status Error status variable. 0 on success.
  * @return the created dma handle
@@ -310,7 +310,7 @@ enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
  *                           timing out
  * @param[in] remainingOut   the number of samples remaining in the queue
  * @param[out] status        Error status variable. 0 on success.
- * @return the succes result of the sample read
+ * @return the success result of the sample read
  */
 enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
                                    HAL_DMASample* dmaSample,

hal/src/main/native/include/hal/DriverStation.h

@@ -28,7 +28,7 @@ extern "C" {
  * @param errorCode the error code
  * @param isLVCode  true for a LV error code, false for a standard error code
  * @param details   the details of the error
- * @param location  the file location of the errror
+ * @param location  the file location of the error
  * @param callStack the callstack of the error
  * @param printMsg  true to print the error message to stdout as well as to the
  * DS

hal/src/main/native/include/hal/Extensions.h

@@ -31,7 +31,7 @@ extern "C" {
  * Expected to be called internally, not by users.
  *
  * @param library the library path
- * @return the succes state of the initialization
+ * @return the success state of the initialization
  */
 int HAL_LoadOneExtension(const char* library);
 
@@ -39,7 +39,7 @@ int HAL_LoadOneExtension(const char* library);
  * Loads any extra halsim libraries provided in the HALSIM_EXTENSIONS
  * environment variable.
  *
- * @return the succes state of the initialization
+ * @return the success state of the initialization
  */
 int HAL_LoadExtensions(void);
 

hal/src/main/native/include/hal/PowerDistribution.h

@@ -36,7 +36,7 @@ extern "C" {
  * Initializes a Power Distribution Panel.
  *
  * @param[in] moduleNumber       the module number to initialize
- * @param[in] type               the type of module to intialize
+ * @param[in] type               the type of module to initialize
  * @param[in] allocationLocation the location where the allocation is occurring
  * @param[out] status            Error status variable. 0 on success.
  * @return the created PowerDistribution

hal/src/main/native/include/hal/SimDevice.h

@@ -47,7 +47,7 @@ extern "C" {
  *
  * The device name must be unique.  0 is returned if the device name already
  * exists.  If multiple instances of the same device are desired, recommend
- * appending the instance/unique identifer in brackets to the base name,
+ * appending the instance/unique identifier in brackets to the base name,
  * e.g. "device[1]".
  *
  * 0 is returned if not in simulation.
@@ -663,7 +663,7 @@ class SimDevice {
    *
    * The device name must be unique.  Returns null if the device name
    * already exists.  If multiple instances of the same device are desired,
-   * recommend appending the instance/unique identifer in brackets to the base
+   * recommend appending the instance/unique identifier in brackets to the base
    * name, e.g. "device[1]".
    *
    * If not in simulation, results in an "empty" object that evaluates to false

hal/src/main/native/include/hal/cpp/UnsafeDIO.h

@@ -63,8 +63,8 @@ int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status);
 
 /**
  * Unsafe digital output set function
- * This function can be used to perform fast and determinstically set digital
- * outputs. This function holds the DIO lock, so calling anyting other then
+ * This function can be used to perform fast and deterministically set digital
+ * outputs. This function holds the DIO lock, so calling anything other then
  * functions on the Proxy object passed as a parameter can deadlock your
  * program.
  *

hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h

@@ -19,7 +19,7 @@ namespace hal {
 
 /**
  * The UnlimitedHandleResource class is a way to track handles. This version
- * allows an unlimted number of handles that are allocated sequentially. When
+ * allows an unlimited number of handles that are allocated sequentially. When
  * possible, indices are reused to save memory usage and keep the array length
  * down.
  * However, automatic array management has not been implemented, but might be in

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

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <algorithm>
+#include <array>
 #include <chrono>
 #include <cmath>
 #include <cstdlib>
@@ -18,6 +19,7 @@
 #include "ntcore_cpp.h"
 
 void bench();
+void bench2();
 void stress();
 
 int main(int argc, char* argv[]) {
@@ -25,6 +27,10 @@ int main(int argc, char* argv[]) {
     bench();
     return EXIT_SUCCESS;
   }
+  if (argc == 2 && std::string_view{argv[1]} == "bench2") {
+    bench2();
+    return EXIT_SUCCESS;
+  }
   if (argc == 2 && std::string_view{argv[1]} == "stress") {
     stress();
     return EXIT_SUCCESS;
@@ -114,6 +120,79 @@ void bench() {
   PrintTimes(flushTimes);
 }
 
+void bench2() {
+  // set up instances
+  auto client1 = nt::CreateInstance();
+  auto client2 = nt::CreateInstance();
+  auto server = nt::CreateInstance();
+
+  // connect client and server
+  nt::StartServer(server, "bench2.json", "127.0.0.1", 10001, 10000);
+  nt::StartClient4(client1, "client1");
+  nt::StartClient3(client2, "client2");
+  nt::SetServer(client1, "127.0.0.1", 10000);
+  nt::SetServer(client2, "127.0.0.1", 10001);
+
+  using namespace std::chrono_literals;
+  std::this_thread::sleep_for(1s);
+
+  // add "typical" set of subscribers on client and server
+  nt::SubscribeMultiple(client1, {{std::string_view{}}});
+  nt::SubscribeMultiple(client2, {{std::string_view{}}});
+  nt::SubscribeMultiple(server, {{std::string_view{}}});
+
+  // create 1000 entries
+  std::array<NT_Entry, 1000> pubs;
+  for (int i = 0; i < 1000; ++i) {
+    pubs[i] = nt::GetEntry(
+        nt::GetTopic(server,
+                     fmt::format("/some/long/name/with/lots/of/slashes/{}", i)),
+        NT_DOUBLE_ARRAY, "double[]");
+  }
+
+  // warm up
+  for (int i = 1; i <= 100; ++i) {
+    for (auto pub : pubs) {
+      double vals[3] = {i * 0.01, i * 0.02, i * 0.03};
+      nt::SetDoubleArray(pub, vals);
+    }
+    nt::FlushLocal(server);
+    std::this_thread::sleep_for(0.02s);
+  }
+
+  std::vector<int64_t> flushTimes;
+  flushTimes.reserve(1001);
+
+  std::vector<int64_t> times;
+  times.reserve(1001);
+
+  // benchmark
+  auto start = std::chrono::high_resolution_clock::now();
+  int64_t now = nt::Now();
+  for (int i = 1; i <= 1000; ++i) {
+    for (auto pub : pubs) {
+      double vals[3] = {i * 0.01, i * 0.02, i * 0.03};
+      nt::SetDoubleArray(pub, vals);
+    }
+    int64_t prev = now;
+    now = nt::Now();
+    times.emplace_back(now - prev);
+    nt::FlushLocal(server);
+    nt::Flush(server);
+    flushTimes.emplace_back(nt::Now() - now);
+    std::this_thread::sleep_for(0.02s);
+    now = nt::Now();
+  }
+  auto stop = std::chrono::high_resolution_clock::now();
+
+  fmt::print("total time: {}us\n",
+             std::chrono::duration_cast<std::chrono::microseconds>(stop - start)
+                 .count());
+  PrintTimes(times);
+  fmt::print("-- Flush --\n");
+  PrintTimes(flushTimes);
+}
+
 static std::random_device r;
 static std::mt19937 gen(r());
 static std::uniform_real_distribution<double> dist;

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

@@ -113,7 +113,7 @@ public final class NetworkTableListener implements AutoCloseable {
    * Create a time synchronization listener.
    *
    * @param inst instance
-   * @param immediateNotify notify listener of current time synchonization value
+   * @param immediateNotify notify listener of current time synchronization value
    * @param listener listener function
    * @return Listener
    */

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

@@ -243,7 +243,7 @@ struct DataLoggerData {
   int Start(TopicData* topic, int64_t time) {
     return log.Start(fmt::format("{}{}", logPrefix,
                                  wpi::drop_front(topic->name, prefix.size())),
-                     topic->typeStr,
+                     topic->typeStr == "int" ? "int64" : topic->typeStr,
                      DataLoggerEntry::MakeMetadata(topic->propertiesStr), time);
   }
 

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

@@ -495,7 +495,7 @@ void NCImpl4::WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp) {
   });
   ws.binary.connect([this](std::span<const uint8_t> data, bool) {
     if (m_clientImpl) {
-      m_clientImpl->ProcessIncomingBinary(data);
+      m_clientImpl->ProcessIncomingBinary(m_loop.Now().count(), data);
     }
   });
 }

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

@@ -53,7 +53,7 @@ class CImpl : public ServerMessageHandler {
             timeSyncUpdated,
         std::function<void(uint32_t repeatMs)> setPeriodic);
 
-  void ProcessIncomingBinary(std::span<const uint8_t> data);
+  void ProcessIncomingBinary(uint64_t curTimeMs, std::span<const uint8_t> data);
   void HandleLocal(std::vector<ClientMessage>&& msgs);
   bool SendControl(uint64_t curTimeMs);
   void SendValues(uint64_t curTimeMs, bool flush);
@@ -91,6 +91,7 @@ class CImpl : public ServerMessageHandler {
   // timestamp handling
   static constexpr uint32_t kPingIntervalMs = 3000;
   uint64_t m_nextPingTimeMs{0};
+  uint64_t m_pongTimeMs{0};
   uint32_t m_rtt2Us{UINT32_MAX};
   bool m_haveTimeOffset{false};
   int64_t m_serverTimeOffsetUs{0};
@@ -125,7 +126,8 @@ CImpl::CImpl(
   m_setPeriodic(m_periodMs);
 }
 
-void CImpl::ProcessIncomingBinary(std::span<const uint8_t> data) {
+void CImpl::ProcessIncomingBinary(uint64_t curTimeMs,
+                                  std::span<const uint8_t> data) {
   for (;;) {
     if (data.empty()) {
       break;
@@ -150,6 +152,7 @@ void CImpl::ProcessIncomingBinary(std::span<const uint8_t> data) {
       }
       DEBUG4("RTT ping response time {} value {}", value.time(),
              value.GetInteger());
+      m_pongTimeMs = curTimeMs;
       int64_t now = wpi::Now();
       int64_t rtt2 = (now - value.GetInteger()) / 2;
       if (rtt2 < m_rtt2Us) {
@@ -207,6 +210,12 @@ bool CImpl::SendControl(uint64_t curTimeMs) {
 
   // start a timestamp RTT ping if it's time to do one
   if (curTimeMs >= m_nextPingTimeMs) {
+    // if we didn't receive a response to our last ping, disconnect
+    if (m_nextPingTimeMs != 0 && m_pongTimeMs == 0) {
+      m_wire.Disconnect("timed out");
+      return false;
+    }
+
     if (!CheckNetworkReady(curTimeMs)) {
       return false;
     }
@@ -215,6 +224,7 @@ bool CImpl::SendControl(uint64_t curTimeMs) {
     WireEncodeBinary(m_wire.SendBinary().Add(), -1, 0, Value::MakeInteger(now));
     // drift isn't critical here, so just go from current time
     m_nextPingTimeMs = curTimeMs + kPingIntervalMs;
+    m_pongTimeMs = 0;
   }
 
   if (!m_outgoing.empty()) {
@@ -465,8 +475,9 @@ void ClientImpl::ProcessIncomingText(std::string_view data) {
   WireDecodeText(data, *m_impl, m_impl->m_logger);
 }
 
-void ClientImpl::ProcessIncomingBinary(std::span<const uint8_t> data) {
-  m_impl->ProcessIncomingBinary(data);
+void ClientImpl::ProcessIncomingBinary(uint64_t curTimeMs,
+                                       std::span<const uint8_t> data) {
+  m_impl->ProcessIncomingBinary(curTimeMs, data);
 }
 
 void ClientImpl::HandleLocal(std::vector<ClientMessage>&& msgs) {

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

@@ -40,7 +40,7 @@ class ClientImpl {
   ~ClientImpl();
 
   void ProcessIncomingText(std::string_view data);
-  void ProcessIncomingBinary(std::span<const uint8_t> data);
+  void ProcessIncomingBinary(uint64_t curTimeMs, std::span<const uint8_t> data);
   void HandleLocal(std::vector<ClientMessage>&& msgs);
 
   void SendControl(uint64_t curTimeMs);

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

@@ -205,6 +205,7 @@ class ClientData4 final : public ClientData4Base {
 
  private:
   std::vector<ServerMessage> m_outgoing;
+  wpi::DenseMap<NT_Topic, size_t> m_outgoingValueMap;
 
   bool WriteBinary(int64_t id, int64_t time, const Value& value) {
     return WireEncodeBinary(SendBinary().Add(), id, time, value);
@@ -282,6 +283,7 @@ class ClientData3 final : public ClientData, private net3::MessageHandler3 {
   net3::WireDecoder3 m_decoder;
 
   std::vector<net3::Message3> m_outgoing;
+  wpi::DenseMap<NT_Topic, size_t> m_outgoingValueMap;
   int64_t m_nextPubUid{1};
 
   struct TopicData3 {
@@ -857,24 +859,23 @@ void ClientData4::SendValue(TopicData* topic, const Value& value,
       }
       break;
     case ClientData::kSendAll:  // append to outgoing
+      m_outgoingValueMap[topic->id] = m_outgoing.size();
       m_outgoing.emplace_back(ServerMessage{ServerValueMsg{topic->id, value}});
       break;
     case ClientData::kSendNormal: {
-      // scan outgoing and replace, or append if not present
-      bool found = false;
-      for (auto&& msg : m_outgoing) {
-        if (auto m = std::get_if<ServerValueMsg>(&msg.contents)) {
-          if (m->topic == topic->id) {
+      // replace, or append if not present
+      auto [it, added] =
+          m_outgoingValueMap.try_emplace(topic->id, m_outgoing.size());
+      if (!added && it->second < m_outgoing.size()) {
+        if (auto m =
+                std::get_if<ServerValueMsg>(&m_outgoing[it->second].contents)) {
+          if (m->topic == topic->id) {  // should always be true
             m->value = value;
-            found = true;
             break;
           }
         }
       }
-      if (!found) {
-        m_outgoing.emplace_back(
-            ServerMessage{ServerValueMsg{topic->id, value}});
-      }
+      m_outgoing.emplace_back(ServerMessage{ServerValueMsg{topic->id, value}});
       break;
     }
   }
@@ -959,6 +960,7 @@ void ClientData4::SendOutgoing(uint64_t curTimeMs) {
     }
   }
   m_outgoing.resize(0);
+  m_outgoingValueMap.clear();
   m_lastSendMs = curTimeMs;
 }
 
@@ -991,6 +993,7 @@ void ClientData3::SendValue(TopicData* topic, const Value& value,
     mode = ClientData::kSendImmNoFlush;  // always send local immediately
   }
   TopicData3* topic3 = GetTopic3(topic);
+  bool added = false;
 
   switch (mode) {
     case ClientData::kSendDisabled:  // do nothing
@@ -1011,24 +1014,26 @@ void ClientData3::SendValue(TopicData* topic, const Value& value,
       }
       break;
     case ClientData::kSendNormal: {
-      // scan outgoing and replace, or append if not present
-      bool found = false;
-      for (auto&& msg : m_outgoing) {
+      // replace, or append if not present
+      wpi::DenseMap<NT_Topic, size_t>::iterator it;
+      std::tie(it, added) =
+          m_outgoingValueMap.try_emplace(topic->id, m_outgoing.size());
+      if (!added && it->second < m_outgoing.size()) {
+        auto& msg = m_outgoing[it->second];
         if (msg.Is(net3::Message3::kEntryUpdate) ||
             msg.Is(net3::Message3::kEntryAssign)) {
-          if (msg.id() == topic->id) {
+          if (msg.id() == topic->id) {  // should always be true
             msg.SetValue(value);
-            found = true;
             break;
           }
         }
       }
-      if (found) {
-        break;
-      }
     }
       // fallthrough
     case ClientData::kSendAll:  // append to outgoing
+      if (!added) {
+        m_outgoingValueMap[topic->id] = m_outgoing.size();
+      }
       ++topic3->seqNum;
       if (topic3->sentAssign) {
         m_outgoing.emplace_back(net3::Message3::EntryUpdate(
@@ -1129,6 +1134,7 @@ void ClientData3::SendOutgoing(uint64_t curTimeMs) {
     net3::WireEncode(out.stream(), msg);
   }
   m_outgoing.resize(0);
+  m_outgoingValueMap.clear();
   m_lastSendMs = curTimeMs;
 }
 
@@ -1287,7 +1293,7 @@ void ClientData3::EntryAssign(std::string_view name, unsigned int id,
   auto topic = m_server.CreateTopic(this, name, typeStr, properties);
   TopicData3* topic3 = GetTopic3(topic);
   if (topic3->published || topic3->sentAssign) {
-    WARNING("ignorning client {} duplicate publish of '{}'", m_id, name);
+    WARNING("ignoring client {} duplicate publish of '{}'", m_id, name);
     return;
   }
   ++topic3->seqNum;

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

@@ -233,7 +233,7 @@ void CImpl::SendPeriodic(uint64_t curTimeMs, bool initial, bool flush) {
 
   auto out = m_wire.Send();
 
-  // send keep-alives
+  // send keep-alive
   if (curTimeMs >= m_nextKeepAliveTimeMs) {
     if (!CheckNetworkReady(curTimeMs)) {
       return;

processstarter/src/main/native/linux/main.cpp

@@ -24,7 +24,7 @@ int main(int argc, char* argv[]) {
     std::perror("readlink");
     return 1;
   } else if (readlink_len == PATH_MAX) {
-    std::printf("Truncation occured\n");
+    std::printf("Truncation occurred\n");
     return 1;
   }
 

test-scripts/deploy-and-run-test-on-robot.sh

@@ -35,7 +35,7 @@ DESTINATION_TEST_RESULTS=none
 LOCAL_TEST_RESULTS=none
 
 
-# Begin searching for options from the second paramater on
+# Begin searching for options from the second parameter on
 PARAM_ARGS=${@:2}
 
 if [[ "$1" = java ]]; then

test-scripts/run-tests-on-robot.sh

@@ -9,7 +9,7 @@
 # This file is intended to be run in the DEFAULT_TEST_DIR on the roboRIO.
 # Do not attempt to run this file on your local system.
 # There is one file (delploy-and-run-test-on-robot.sh) that is designed to
-# deploy this file allong with the compiled tests for you.
+# deploy this file along with the compiled tests for you.
 
 # Configurable variables
 source config.sh
@@ -24,9 +24,9 @@ Where:
     -h    Show this help text
     -d    The directory where the tests have been placed.
           This is done to prevent overwriting an already running program.
-          This scrip will automatically move the test into the ${DEFAULT_TEST_DIR}
+          This script will automatically move the test into the ${DEFAULT_TEST_DIR}
           directory.
-          Default: Assumes the test is in the same directory as this scrip.
+          Default: Assumes the test is in the same directory as this script.
     -A    Do not use the default arguments for the given language.
     arg   The arguments to be passed to test."
 
@@ -42,7 +42,7 @@ fi
 LANGUAGE=none
 TEST_FILE=none
 TEST_DIR="$DEFAULT_TEST_DIR"
-# Begin searching for options from the second paramater on
+# Begin searching for options from the second parameter on
 PARAM_ARGS=${@:2}
 # Where the test arguments start
 TEST_RUN_ARGS=${@:2}
@@ -125,7 +125,7 @@ elif [[ $TEST_DIR != "$DEFAULT_TEST_DIR" ]]; then
     mv "${TEST_DIR}/${TEST_FILE}" "${DEFAULT_TEST_DIR}"
 fi
 
-# Make sure the excecutable file has permission to run
+# Make sure the executable file has permission to run
 
 # Get the serial number and FPGADeviceCode for this rio
 export serialnum=`/sbin/fw_printenv -n serial#`

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

@@ -626,7 +626,6 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
    *
    * @param command The command to check
    * @return true if composed
-   * @throws IllegalArgumentException if the given commands have already been composed.
    */
   public boolean isComposed(Command command) {
     return getComposedCommands().contains(command);

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

@@ -45,7 +45,7 @@ public class RamseteCommand extends CommandBase {
   private final PIDController m_leftController;
   private final PIDController m_rightController;
   private final BiConsumer<Double, Double> m_output;
-  private DifferentialDriveWheelSpeeds m_prevSpeeds;
+  private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds();
   private double m_prevTime;
 
   /**

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

@@ -9,8 +9,7 @@ import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Timer;
 
 /**
- * A command that does nothing but takes a specified amount of time to finish. Useful for
- * CommandGroups. Can also be subclassed to make a command with an internal timer.
+ * A command that does nothing but takes a specified amount of time to finish.
  *
  * <p>This class is provided by the NewCommands VendorDep
  */

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

@@ -13,8 +13,6 @@
 namespace frc2 {
 /**
  * A command that does nothing but takes a specified amount of time to finish.
- * Useful for CommandGroups.  Can also be subclassed to make a command with an
- * internal timer.
  *
  * This class is provided by the NewCommands VendorDep
  */

wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp

@@ -285,7 +285,7 @@ bool ADIS16448_IMU::SwitchToAutoSPI() {
   m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
   // Configure auto stall time
   m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
-  // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+  // Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
   // activated)
   m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
   // Check to see if the acquire thread is running. If not, kick one off.

wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp

@@ -279,7 +279,7 @@ bool ADIS16470_IMU::SwitchToAutoSPI() {
   }
   // Configure auto stall time
   m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
-  // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+  // Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
   // activated) DR High = Data good (data capture should be triggered on the
   // rising edge)
   m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);

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

@@ -186,7 +186,7 @@ void Thread::Main() {
       } else {
         dsAttachCount = 0;
       }
-      if (dsAttachCount > 50) {  // 1 second
+      if (dsAttachCount > 300) {  // 6 seconds
         std::time_t now = std::time(nullptr);
         auto tm = std::gmtime(&now);
         if (tm->tm_year > 100) {
@@ -207,7 +207,7 @@ void Thread::Main() {
       } else {
         fmsAttachCount = 0;
       }
-      if (fmsAttachCount > 100) {  // 2 seconds
+      if (fmsAttachCount > 250) {  // 5 seconds
         // match info comes through TCP, so we need to double-check we've
         // actually received it
         auto matchType = DriverStation::GetMatchType();

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -484,6 +484,12 @@ bool DriverStation::IsTest() {
   return controlWord.test;
 }
 
+bool DriverStation::IsTestEnabled() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.test && controlWord.enabled;
+}
+
 bool DriverStation::IsDSAttached() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -95,7 +95,7 @@ void IterativeRobotBase::SetNetworkTablesFlushEnabled(bool enabled) {
 }
 
 void IterativeRobotBase::EnableLiveWindowInTest(bool testLW) {
-  if (IsTest()) {
+  if (IsTestEnabled()) {
     throw FRC_MakeError(err::IncompatibleMode,
                         "Can't configure test mode while in test mode!");
   }

wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp

@@ -20,12 +20,16 @@ struct ShuffleboardInstance::Impl {
   bool tabsChanged = false;
   std::shared_ptr<nt::NetworkTable> rootTable;
   std::shared_ptr<nt::NetworkTable> rootMetaTable;
+  nt::StringPublisher selectedTabPub;
 };
 
 ShuffleboardInstance::ShuffleboardInstance(nt::NetworkTableInstance ntInstance)
     : m_impl(new Impl) {
   m_impl->rootTable = ntInstance.GetTable(Shuffleboard::kBaseTableName);
   m_impl->rootMetaTable = m_impl->rootTable->GetSubTable(".metadata");
+  m_impl->selectedTabPub =
+      m_impl->rootMetaTable->GetStringTopic("Selected")
+          .Publish(nt::PubSubOptions{.keepDuplicates = true});
   HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
 }
 
@@ -75,9 +79,9 @@ void ShuffleboardInstance::DisableActuatorWidgets() {
 }
 
 void ShuffleboardInstance::SelectTab(int index) {
-  m_impl->rootMetaTable->GetEntry("Selected").SetDouble(index);
+  m_impl->selectedTabPub.Set(std::to_string(index));
 }
 
 void ShuffleboardInstance::SelectTab(std::string_view title) {
-  m_impl->rootMetaTable->GetEntry("Selected").SetString(title);
+  m_impl->selectedTabPub.Set(title);
 }

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

@@ -210,6 +210,10 @@ bool RobotBase::IsTest() const {
   return DriverStation::IsTest();
 }
 
+bool RobotBase::IsTestEnabled() const {
+  return DriverStation::IsTestEnabled();
+}
+
 std::thread::id RobotBase::GetThreadId() {
   return m_threadId;
 }
@@ -225,7 +229,7 @@ RobotBase::RobotBase() {
   SetupMathShared();
 
   auto inst = nt::NetworkTableInstance::GetDefault();
-  // subscribe to "" to force persistent values to progagate to local
+  // subscribe to "" to force persistent values to propagate to local
   nt::SubscribeMultiple(inst.GetHandle(), {{std::string_view{}}});
 #ifdef __FRC_ROBORIO__
   inst.StartServer("/home/lvuser/networktables.json");

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

@@ -30,7 +30,7 @@ class AnalogTrigger;
  * range defined by the limits.
  *
  * The RisingPulse and FallingPulse outputs detect an instantaneous transition
- * from above the upper limit to below the lower limit, and vise versa. These
+ * from above the upper limit to below the lower limit, and vice versa. These
  * pulses represent a rollover condition of a sensor and can be routed to an up
  * / down counter or to interrupts. Because the outputs generate a pulse, they
  * cannot be read directly. To help ensure that a rollover condition is not

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

@@ -209,6 +209,14 @@ class DriverStation final {
    */
   static bool IsTest();
 
+  /**
+   * Check if the DS is commanding Test mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in Test mode and
+   * enabled.
+   */
+  static bool IsTestEnabled();
+
   /**
    * Check if the DS is attached.
    *

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

@@ -149,7 +149,7 @@ class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
   virtual double GetSpeed() const;
 
   /**
-   * Temporarily disables the PWM output. The next set call will reenable
+   * Temporarily disables the PWM output. The next set call will re-enable
    * the output.
    */
   virtual void SetDisabled();

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

@@ -17,7 +17,7 @@ namespace frc {
 /**
  * The Resource class is a convenient way to track allocated resources.
  *
- * It tracks them as indicies in the range [0 .. elements - 1]. E.g. the library
+ * It tracks them as indices in the range [0 .. elements - 1]. E.g. the library
  * uses this to track hardware channel allocation.
  *
  * The Resource class does not allocate the hardware channels or other
@@ -46,7 +46,7 @@ class Resource {
    * Allocate storage for a new instance of Resource.
    *
    * Allocate a bool array of values that will get initialized to indicate that
-   * no resources have been allocated yet. The indicies of the resources are
+   * no resources have been allocated yet. The indices of the resources are
    * [0 .. elements - 1].
    */
   explicit Resource(uint32_t size);

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

@@ -129,14 +129,14 @@ class RobotBase {
   /**
    * Determine if the Robot is currently enabled.
    *
-   * @return True if the Robot is currently enabled by the field controls.
+   * @return True if the Robot is currently enabled by the Driver Station.
    */
   bool IsEnabled() const;
 
   /**
    * Determine if the Robot is currently disabled.
    *
-   * @return True if the Robot is currently disabled by the field controls.
+   * @return True if the Robot is currently disabled by the Driver Station.
    */
   bool IsDisabled() const;
 
@@ -144,7 +144,7 @@ class RobotBase {
    * Determine if the robot is currently in Autonomous mode.
    *
    * @return True if the robot is currently operating Autonomously as determined
-   *         by the field controls.
+   *         by the Driver Station.
    */
   bool IsAutonomous() const;
 
@@ -152,7 +152,7 @@ class RobotBase {
    * Determine if the robot is currently in Autonomous mode and enabled.
    *
    * @return True if the robot us currently operating Autonomously while enabled
-   * as determined by the field controls.
+   * as determined by the Driver Station.
    */
   bool IsAutonomousEnabled() const;
 
@@ -160,7 +160,7 @@ class RobotBase {
    * Determine if the robot is currently in Operator Control mode.
    *
    * @return True if the robot is currently operating in Tele-Op mode as
-   *         determined by the field controls.
+   *         determined by the Driver Station.
    */
   bool IsTeleop() const;
 
@@ -168,18 +168,26 @@ class RobotBase {
    * Determine if the robot is current in Operator Control mode and enabled.
    *
    * @return True if the robot is currently operating in Tele-Op mode while
-   * wnabled as determined by the field-controls.
+   * enabled as determined by the Driver Station.
    */
   bool IsTeleopEnabled() const;
 
   /**
    * Determine if the robot is currently in Test mode.
    *
-   * @return True if the robot is currently running tests as determined by the
-   *         field controls.
+   * @return True if the robot is currently running in Test mode as determined
+   * by the Driver Station.
    */
   bool IsTest() const;
 
+  /**
+   * Determine if the robot is current in Test mode and enabled.
+   *
+   * @return True if the robot is currently operating in Test mode while
+   * enabled as determined by the Driver Station.
+   */
+  bool IsTestEnabled() const;
+
   /**
    * Gets the ID of the main robot thread.
    */

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

@@ -16,7 +16,7 @@ namespace frc {
  * Driver for the RS-232 serial port on the roboRIO.
  *
  * The current implementation uses the VISA formatted I/O mode.  This means that
- * all traffic goes through the fomatted buffers.  This allows the intermingled
+ * all traffic goes through the formatted buffers.  This allows the intermingled
  * use of Printf(), Scanf(), and the raw buffer accessors Read() and Write().
  *
  * More information can be found in the NI-VISA User Manual here:

wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp

@@ -7,6 +7,8 @@
 #include <string_view>
 
 #include <networktables/NetworkTableInstance.h>
+#include <networktables/NetworkTableListener.h>
+#include <networktables/StringTopic.h>
 
 #include "frc/shuffleboard/ShuffleboardInstance.h"
 #include "gtest/gtest.h"
@@ -106,3 +108,22 @@ TEST(ShuffleboardInstanceTest, NestedActuatorWidgetsAreDisabled) {
   EXPECT_FALSE(controllable)
       << "The nested actuator widget should have been disabled";
 }
+
+TEST(ShuffleboardInstanceTest, DuplicateSelectTabs) {
+  NTWrapper ntInst;
+  frc::detail::ShuffleboardInstance shuffleboardInst{ntInst.inst};
+  std::atomic_int counter = 0;
+  auto listener = nt::NetworkTableListener::CreateListener(
+      ntInst.inst.GetStringTopic("/Shuffleboard/.metadata/Selected"),
+      nt::EventFlags::kValueAll | nt::EventFlags::kImmediate,
+      [&counter](auto& event) { counter++; });
+
+  // There shouldn't be anything there
+  EXPECT_EQ(0, counter);
+
+  shuffleboardInst.SelectTab("tab1");
+  shuffleboardInst.SelectTab("tab1");
+  EXPECT_TRUE(ntInst.inst.WaitForListenerQueue(0.005))
+      << "Listener queue timed out!";
+  EXPECT_EQ(2, counter);
+}

wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp

@@ -66,7 +66,7 @@ TEST(ElevatorSimTest, MinMax) {
 
 TEST(ElevatorSimTest, Stability) {
   frc::sim::ElevatorSim sim{
-      frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, true};
+      frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false};
 
   sim.SetState(frc::Vectord<2>{0.0, 0.0});
   sim.SetInput(frc::Vectord<1>{12.0});

wpilibcExamples/src/main/cpp/examples/RomiReference/include/RobotContainer.h

@@ -41,7 +41,7 @@ class RobotContainer {
   frc2::Command* GetAutonomousCommand();
 
  private:
-  // Assumes a gamepad plugged into channnel 0
+  // Assumes a gamepad plugged into channel 0
   frc::Joystick m_controller{0};
   frc::SendableChooser<frc2::Command*> m_chooser;
 

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

@@ -42,7 +42,7 @@ class Robot : public frc::TimedRobot {
 
     // Put encoders in a list layout.
     frc::ShuffleboardLayout& encoders =
-        driveBaseTab.GetLayout("List Layout", "Encoders")
+        driveBaseTab.GetLayout("Encoders", frc::BuiltInLayouts::kList)
             .WithPosition(0, 0)
             .WithSize(2, 2);
     encoders.Add("Left Encoder", m_leftEncoder);

wpilibcIntegrationTests/src/main/native/cpp/DigitalGlitchFilterTest.cpp

@@ -14,7 +14,7 @@
  *
  * This test actually tests everything except that the actual FPGA
  * implementation works as intended.  We configure the FPGA and then query it to
- * make sure that the acutal configuration matches.
+ * make sure that the actual configuration matches.
  */
 TEST(DigitalGlitchFilterTest, Basic) {
   frc::DigitalInput input1{1};

wpilibcIntegrationTests/src/main/native/cpp/FakeEncoderTest.cpp

@@ -62,7 +62,7 @@ class FakeEncoderTest : public testing::Test {
 };
 
 /**
- * Test the encoder by reseting it to 0 and reading the value.
+ * Test the encoder by resetting it to 0 and reading the value.
  */
 TEST_F(FakeEncoderTest, DefaultState) {
   EXPECT_DOUBLE_EQ(0.0, m_encoder.Get()) << "The encoder did not start at 0.";

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

@@ -455,7 +455,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
     m_spi.setAutoTransmitData(new byte[] {GLOB_CMD}, 27);
     // Configure auto stall time
     m_spi.configureAutoStall(100, 1000, 255);
-    // Kick off auto SPI (Note: Device configration impossible after auto SPI is
+    // Kick off auto SPI (Note: Device configuration impossible after auto SPI is
     // activated)
     m_spi.startAutoTrigger(m_auto_interrupt, true, false);
 

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

@@ -511,7 +511,7 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable {
     }
     // Configure auto stall time
     m_spi.configureAutoStall(5, 1000, 1);
-    // Kick off auto SPI (Note: Device configration impossible after auto SPI is
+    // Kick off auto SPI (Note: Device configuration impossible after auto SPI is
     // activated)
     // DR High = Data good (data capture should be triggered on the rising edge)
     m_spi.startAutoTrigger(m_auto_interrupt, true, false);

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

@@ -26,7 +26,7 @@ import edu.wpi.first.util.sendable.SendableBuilder;
  * limits.
  *
  * <p>The RisingPulse and FallingPulse outputs detect an instantaneous transition from above the
- * upper limit to below the lower limit, and vise versa. These pulses represent a rollover condition
+ * upper limit to below the lower limit, and vice versa. These pulses represent a rollover condition
  * of a sensor and can be routed to an up / down counter or to interrupts. Because the outputs
  * generate a pulse, they cannot be read directly. To help ensure that a rollover condition is not
  * missed, there is an average rejection filter available that operates on the upper 8 bits of a 12

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

@@ -316,7 +316,7 @@ public final class DataLogManager {
         } else {
           dsAttachCount = 0;
         }
-        if (dsAttachCount > 50) { // 1 second
+        if (dsAttachCount > 300) { // 6 seconds
           LocalDateTime now = LocalDateTime.now(m_utc);
           if (now.getYear() > 2000) {
             // assume local clock is now synchronized to DS, so rename based on
@@ -336,7 +336,7 @@ public final class DataLogManager {
         } else {
           fmsAttachCount = 0;
         }
-        if (fmsAttachCount > 100) { // 2 seconds
+        if (fmsAttachCount > 250) { // 5 seconds
           // match info comes through TCP, so we need to double-check we've
           // actually received it
           DriverStation.MatchType matchType = DriverStation.getMatchType();

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

@@ -216,78 +216,97 @@ public final class DriverStation {
       m_logAxes = new FloatArrayLogEntry(log, "DS:joystick" + stick + "/axes", timestamp);
       m_logPOVs = new IntegerArrayLogEntry(log, "DS:joystick" + stick + "/povs", timestamp);
 
-      appendButtons(timestamp);
-      appendAxes(timestamp);
-      appendPOVs(timestamp);
+      appendButtons(m_joystickButtons[m_stick], timestamp);
+      appendAxes(m_joystickAxes[m_stick], timestamp);
+      appendPOVs(m_joystickPOVs[m_stick], timestamp);
     }
 
     public void send(long timestamp) {
-      if (m_joystickButtonsCache[m_stick].m_count != m_joystickButtons[m_stick].m_count
-          || m_joystickButtonsCache[m_stick].m_buttons != m_joystickButtons[m_stick].m_buttons) {
-        appendButtons(timestamp);
+      HALJoystickButtons buttons = m_joystickButtons[m_stick];
+      if (buttons.m_count != m_prevButtons.m_count
+          || buttons.m_buttons != m_prevButtons.m_buttons) {
+        appendButtons(buttons, timestamp);
       }
 
-      if (m_joystickAxesCache[m_stick].m_count != m_joystickAxes[m_stick].m_count) {
-        appendAxes(timestamp);
+      HALJoystickAxes axes = m_joystickAxes[m_stick];
+      int count = axes.m_count;
+      boolean needToLog = false;
+      if (count != m_prevAxes.m_count) {
+        needToLog = true;
       } else {
-        int count = m_joystickAxesCache[m_stick].m_count;
         for (int i = 0; i < count; i++) {
-          if (m_joystickAxesCache[m_stick].m_axes[i] != m_joystickAxes[m_stick].m_axes[i]) {
-            appendAxes(timestamp);
-            break;
+          if (axes.m_axes[i] != m_prevAxes.m_axes[i]) {
+            needToLog = true;
           }
         }
       }
+      if (needToLog) {
+        appendAxes(axes, timestamp);
+      }
 
-      if (m_joystickPOVsCache[m_stick].m_count != m_joystickPOVs[m_stick].m_count) {
-        appendPOVs(timestamp);
+      HALJoystickPOVs povs = m_joystickPOVs[m_stick];
+      count = m_joystickPOVs[m_stick].m_count;
+      needToLog = false;
+      if (count != m_prevPOVs.m_count) {
+        needToLog = true;
       } else {
-        int count = m_joystickPOVsCache[m_stick].m_count;
         for (int i = 0; i < count; i++) {
-          if (m_joystickPOVsCache[m_stick].m_povs[i] != m_joystickPOVs[m_stick].m_povs[i]) {
-            appendPOVs(timestamp);
-            break;
+          if (povs.m_povs[i] != m_prevPOVs.m_povs[i]) {
+            needToLog = true;
           }
         }
       }
+      if (needToLog) {
+        appendPOVs(povs, timestamp);
+      }
     }
 
-    void appendButtons(long timestamp) {
-      int count = m_joystickButtonsCache[m_stick].m_count;
+    void appendButtons(HALJoystickButtons buttons, long timestamp) {
+      byte count = buttons.m_count;
       if (m_sizedButtons == null || m_sizedButtons.length != count) {
         m_sizedButtons = new boolean[count];
       }
-      int buttons = m_joystickButtonsCache[m_stick].m_buttons;
+      int buttonsValue = buttons.m_buttons;
       for (int i = 0; i < count; i++) {
-        m_sizedButtons[i] = (buttons & (1 << i)) != 0;
+        m_sizedButtons[i] = (buttonsValue & (1 << i)) != 0;
       }
       m_logButtons.append(m_sizedButtons, timestamp);
+      m_prevButtons.m_count = count;
+      m_prevButtons.m_buttons = buttons.m_buttons;
     }
 
-    void appendAxes(long timestamp) {
-      int count = m_joystickAxesCache[m_stick].m_count;
+    void appendAxes(HALJoystickAxes axes, long timestamp) {
+      int count = axes.m_count;
       if (m_sizedAxes == null || m_sizedAxes.length != count) {
         m_sizedAxes = new float[count];
       }
-      System.arraycopy(m_joystickAxesCache[m_stick].m_axes, 0, m_sizedAxes, 0, count);
+      System.arraycopy(axes.m_axes, 0, m_sizedAxes, 0, count);
       m_logAxes.append(m_sizedAxes, timestamp);
+      m_prevAxes.m_count = count;
+      System.arraycopy(axes.m_axes, 0, m_prevAxes.m_axes, 0, count);
     }
 
-    void appendPOVs(long timestamp) {
-      int count = m_joystickPOVsCache[m_stick].m_count;
+    @SuppressWarnings("PMD.AvoidArrayLoops")
+    void appendPOVs(HALJoystickPOVs povs, long timestamp) {
+      int count = povs.m_count;
       if (m_sizedPOVs == null || m_sizedPOVs.length != count) {
         m_sizedPOVs = new long[count];
       }
       for (int i = 0; i < count; i++) {
-        m_sizedPOVs[i] = m_joystickPOVsCache[m_stick].m_povs[i];
+        m_sizedPOVs[i] = povs.m_povs[i];
       }
       m_logPOVs.append(m_sizedPOVs, timestamp);
+      m_prevPOVs.m_count = count;
+      System.arraycopy(povs.m_povs, 0, m_prevPOVs.m_povs, 0, count);
     }
 
     final int m_stick;
     boolean[] m_sizedButtons;
     float[] m_sizedAxes;
     long[] m_sizedPOVs;
+    final HALJoystickButtons m_prevButtons = new HALJoystickButtons();
+    final HALJoystickAxes m_prevAxes = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
+    final HALJoystickPOVs m_prevPOVs = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
     final BooleanArrayLogEntry m_logButtons;
     final FloatArrayLogEntry m_logAxes;
     final IntegerArrayLogEntry m_logPOVs;
@@ -927,7 +946,7 @@ public final class DriverStation {
   }
 
   /**
-   * Gets a value indicating whether the Driver Station requires the robot to be running in test
+   * Gets a value indicating whether the Driver Station requires the robot to be running in Test
    * mode.
    *
    * @return True if test mode should be enabled, false otherwise.
@@ -941,6 +960,21 @@ public final class DriverStation {
     }
   }
 
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in Test
+   * mode and enabled.
+   *
+   * @return True if test mode should be set and the robot should be enabled.
+   */
+  public static boolean isTestEnabled() {
+    m_cacheDataMutex.lock();
+    try {
+      return m_controlWord.getTest() && m_controlWord.getEnabled();
+    } finally {
+      m_cacheDataMutex.unlock();
+    }
+  }
+
   /**
    * Gets a value indicating whether the Driver Station is attached.
    *

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

@@ -208,6 +208,9 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
    */
   public void setDistancePerRotation(double distancePerRotation) {
     m_distancePerRotation = distancePerRotation;
+    if (m_simDistancePerRotation != null) {
+      m_simDistancePerRotation.set(distancePerRotation);
+    }
   }
 
   /**

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

@@ -253,7 +253,7 @@ public abstract class IterativeRobotBase extends RobotBase {
    * @throws ConcurrentModificationException if this is called during test mode.
    */
   public void enableLiveWindowInTest(boolean testLW) {
-    if (isTest()) {
+    if (isTestEnabled()) {
       throw new ConcurrentModificationException("Can't configure test mode while in test mode!");
     }
     m_lwEnabledInTest = testLW;

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

@@ -216,7 +216,7 @@ public abstract class RobotBase implements AutoCloseable {
   /**
    * Determine if the Robot is currently disabled.
    *
-   * @return True if the Robot is currently disabled by the field controls.
+   * @return True if the Robot is currently disabled by the Driver Station.
    */
   public boolean isDisabled() {
     return DriverStation.isDisabled();
@@ -225,14 +225,14 @@ public abstract class RobotBase implements AutoCloseable {
   /**
    * Determine if the Robot is currently enabled.
    *
-   * @return True if the Robot is currently enabled by the field controls.
+   * @return True if the Robot is currently enabled by the Driver Station.
    */
   public boolean isEnabled() {
     return DriverStation.isEnabled();
   }
 
   /**
-   * Determine if the robot is currently in Autonomous mode as determined by the field controls.
+   * Determine if the robot is currently in Autonomous mode as determined by the Driver Station.
    *
    * @return True if the robot is currently operating Autonomously.
    */
@@ -241,8 +241,8 @@ public abstract class RobotBase implements AutoCloseable {
   }
 
   /**
-   * Determine if the robot is current in Autonomous mode and enabled as determined by the field
-   * controls.
+   * Determine if the robot is currently in Autonomous mode and enabled as determined by the Driver
+   * Station.
    *
    * @return True if the robot is currently operating autonomously while enabled.
    */
@@ -251,7 +251,7 @@ public abstract class RobotBase implements AutoCloseable {
   }
 
   /**
-   * Determine if the robot is currently in Test mode as determined by the driver station.
+   * Determine if the robot is currently in Test mode as determined by the Driver Station.
    *
    * @return True if the robot is currently operating in Test mode.
    */
@@ -260,8 +260,17 @@ public abstract class RobotBase implements AutoCloseable {
   }
 
   /**
-   * Determine if the robot is currently in Operator Control mode as determined by the field
-   * controls.
+   * Determine if the robot is current in Test mode and enabled as determined by the Driver Station.
+   *
+   * @return True if the robot is currently operating in Test mode while enabled.
+   */
+  public boolean isTestEnabled() {
+    return DriverStation.isTestEnabled();
+  }
+
+  /**
+   * Determine if the robot is currently in Operator Control mode as determined by the Driver
+   * Station.
    *
    * @return True if the robot is currently operating in Tele-Op mode.
    */
@@ -270,8 +279,8 @@ public abstract class RobotBase implements AutoCloseable {
   }
 
   /**
-   * Determine if the robot is current in Operator Control mode and enabled as determined by the
-   * field controls.
+   * Determine if the robot is currently in Operator Control mode and enabled as determined by the
+   * Driver Station.
    *
    * @return True if the robot is currently operating in Tele-Op mode while enabled.
    */

wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java

@@ -86,7 +86,7 @@ public final class LiveWindow {
    * enable all the components registered for LiveWindow. If it's being disabled, stop all the
    * registered components and re-enable the scheduler.
    *
-   * <p>TODO: add code to disable PID loops when enabling LiveWindow. The commands should reenable
+   * <p>TODO: add code to disable PID loops when enabling LiveWindow. The commands should re-enable
    * the PID loops themselves when they get rescheduled. This prevents arms from starting to move
    * around, etc. after a period of adjusting them in LiveWindow mode.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java

@@ -10,6 +10,7 @@ import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.StringPublisher;
 import java.util.LinkedHashMap;
 import java.util.Map;
@@ -32,7 +33,8 @@ final class ShuffleboardInstance implements ShuffleboardRoot {
     requireNonNullParam(ntInstance, "ntInstance", "ShuffleboardInstance");
     m_rootTable = ntInstance.getTable(Shuffleboard.kBaseTableName);
     m_rootMetaTable = m_rootTable.getSubTable(".metadata");
-    m_selectedTabPub = m_rootMetaTable.getStringTopic("Selected").publish();
+    m_selectedTabPub =
+        m_rootMetaTable.getStringTopic("Selected").publish(PubSubOption.keepDuplicates(true));
     HAL.report(tResourceType.kResourceType_Shuffleboard, 0);
   }
 

wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java

@@ -11,7 +11,10 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableEvent.Kind;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import java.util.EnumSet;
+import java.util.concurrent.atomic.AtomicInteger;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -121,4 +124,28 @@ class ShuffleboardInstanceTest {
     controllable = controllableEntry.getValue().getBoolean();
     assertFalse(controllable, "The nested actuator widget should have been disabled");
   }
+
+  @Test
+  void testDuplicateSelectTabs() {
+    int listener = 0;
+    AtomicInteger counter = new AtomicInteger();
+    try {
+      listener =
+          m_ntInstance.addListener(
+              m_ntInstance.getStringTopic("/Shuffleboard/.metadata/Selected"),
+              EnumSet.of(Kind.kValueAll, Kind.kImmediate),
+              event -> counter.incrementAndGet());
+
+      // There shouldn't be anything there
+      assertEquals(0, counter.get());
+
+      m_shuffleboardInstance.selectTab("tab1");
+      m_shuffleboardInstance.selectTab("tab1");
+      assertTrue(m_ntInstance.waitForListenerQueue(0.005), "Listener queue timed out!");
+      assertEquals(2, counter.get());
+
+    } finally {
+      m_ntInstance.removeListener(listener);
+    }
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -91,7 +91,7 @@ class ElevatorSimTest {
   @Test
   void testStability() {
     var sim =
-        new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, true);
+        new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false);
 
     sim.setState(VecBuilder.fill(0, 0));
     sim.setInput(12);

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

@@ -131,7 +131,7 @@
   },
   {
     "name": "TrapezoidProfileSubsystem",
-    "description": "A subystem that executes a trapezoidal motion profile.",
+    "description": "A subsystem that executes a trapezoidal motion profile.",
     "tags": [
       "trapezoidprofilesubsystem"
     ],

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

@@ -124,7 +124,7 @@ public class Robot extends TimedRobot {
         // determine pose
         Transform3d pose = estimator.estimate(detection);
 
-        // put pose into dashbaord
+        // put pose into dashboard
         Rotation3d rot = pose.getRotation();
         tagsTable
             .getEntry("pose_" + detection.getId())

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java

@@ -103,7 +103,7 @@ public class RobotContainer {
             DriveConstants.kFeedforward,
             DriveConstants.kDriveKinematics,
 
-            // Position contollers
+            // Position controllers
             new PIDController(AutoConstants.kPXController, 0, 0),
             new PIDController(AutoConstants.kPYController, 0, 0),
             new ProfiledPIDController(

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java

@@ -30,7 +30,7 @@ public class RobotContainer {
   private final Drivetrain m_drivetrain = new Drivetrain();
   private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
 
-  // Assumes a gamepad plugged into channnel 0
+  // Assumes a gamepad plugged into channel 0
   private final Joystick m_controller = new Joystick(0);
 
   // Create SmartDashboard chooser for autonomous routines

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

@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -41,7 +42,7 @@ public class Robot extends TimedRobot {
     driveBaseTab.add("Tank Drive", m_tankDrive);
     // Put both encoders in a list layout
     ShuffleboardLayout encoders =
-        driveBaseTab.getLayout("List Layout", "Encoders").withPosition(0, 0).withSize(2, 2);
+        driveBaseTab.getLayout("Encoders", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 2);
     encoders.add("Left Encoder", m_leftEncoder);
     encoders.add("Right Encoder", m_rightEncoder);
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java

@@ -13,8 +13,8 @@ public class SampleFixture implements ITestFixture {
   public void setup() {
     /*
      * If this fixture actually accessed the hardware, here is where it would
-     * set up the starting state of the test bench. For example, reseting
-     * encoders, ensuring motors are stopped, reseting any serial communication
+     * set up the starting state of the test bench. For example, resetting
+     * encoders, ensuring motors are stopped, resetting any serial communication
      * if necessary, etc.
      */
   }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java

@@ -72,7 +72,7 @@ public class FakeCounterSource implements AutoCloseable {
     }
   }
 
-  /** Common initailization code. */
+  /** Common initialization code. */
   private void initEncoder() {
     m_milliSec = 1;
     m_task = new EncoderThread(this);
@@ -95,7 +95,7 @@ public class FakeCounterSource implements AutoCloseable {
     Timer.delay(0.01);
   }
 
-  /** Starts and completes a task set - does not return until thred has finished its operations. */
+  /** Starts and completes a task set - does not return until thread has finished its operations. */
   public void execute() {
     start();
     complete();

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakePotentiometerSource.java

@@ -75,7 +75,7 @@ public class FakePotentiometerSource implements AutoCloseable {
     return voltage * (m_potMaxAngle / m_potMaxVoltage);
   }
 
-  /** Frees the resouce. */
+  /** Frees the resource. */
   @Override
   public void close() {
     if (m_initOutput) {

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java

@@ -60,9 +60,9 @@ public class AntJunitLauncher {
         FormatterElement.TypeAttribute type = new FormatterElement.TypeAttribute();
         type.setValue("xml");
 
-        FormatterElement formater = new FormatterElement();
-        formater.setType(type);
-        task.addFormatter(formater);
+        FormatterElement formatter = new FormatterElement();
+        formatter.setType(type);
+        task.addFormatter(formatter);
 
         // Create the JUnitTest
         JUnitTest test = new JUnitTest(TestSuite.class.getName());

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

@@ -222,7 +222,7 @@ public class Matrix<R extends Num, C extends Num> {
    *
    * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
    *
-   * @param other The other {@link Matrix} to preform element multiplication on.
+   * @param other The other {@link Matrix} to perform element multiplication on.
    * @return The element by element multiplication of "this" and other.
    */
   public final Matrix<R, C> elementTimes(Matrix<R, C> other) {

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

@@ -61,6 +61,7 @@ public class LTVDifferentialDriveController {
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0.
    */
   public LTVDifferentialDriveController(
       LinearSystem<N2, N2, N2> plant,
@@ -127,6 +128,11 @@ public class LTVDifferentialDriveController {
             .times(-1.0)
             .get(0, 0);
 
+    if (maxV <= 0.0) {
+      throw new IllegalArgumentException(
+          "Max velocity of plant with 12 V input must be greater than zero.");
+    }
+
     for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
       // The DARE is ill-conditioned if the velocity is close to zero, so don't
       // let the system stop.

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

@@ -66,6 +66,7 @@ public class LTVUnicycleController {
    * @param dt Discretization timestep in seconds.
    * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
    *     table. The default is 9 m/s.
+   * @throws IllegalArgumentException if maxVelocity <= 0.
    */
   public LTVUnicycleController(double dt, double maxVelocity) {
     this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
@@ -90,9 +91,14 @@ public class LTVUnicycleController {
    * @param dt Discretization timestep in seconds.
    * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
    *     table. The default is 9 m/s.
+   * @throws IllegalArgumentException if maxVelocity <= 0.
    */
   public LTVUnicycleController(
       Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+    if (maxVelocity <= 0.0) {
+      throw new IllegalArgumentException("Max velocity must be greater than zero.");
+    }
+
     // The change in global pose for a unicycle is defined by the following
     // three equations.
     //

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

@@ -212,7 +212,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
     double A;
     double B;
     double C;
-    if (Math.abs(theta) < 1E-9) {
+    if (Math.abs(theta) < 1E-7) {
       // Taylor Expansions around θ = 0
       // A = 1/1! - θ²/3! + θ⁴/5!
       // B = 1/2! - θ²/4! + θ⁴/6!
@@ -268,7 +268,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
     final var thetaSq = theta * theta;
 
     double C;
-    if (Math.abs(theta) < 1E-9) {
+    if (Math.abs(theta) < 1E-7) {
       // Taylor Expansions around θ = 0
       // A = 1/1! - θ²/3! + θ⁴/5!
       // B = 1/2! - θ²/4! + θ⁴/6!

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

@@ -8,7 +8,7 @@ import java.util.Objects;
 
 /**
  * A change in distance along a 2D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose2d objects from a Twist2d and vise versa.
+ * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */

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

@@ -8,7 +8,7 @@ import java.util.Objects;
 
 /**
  * A change in distance along a 3D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose3d objects from a Twist3d and vise versa.
+ * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */

wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java

@@ -71,8 +71,8 @@ public final class SplineParameterizer {
   private SplineParameterizer() {}
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
-   * dy, and dtheta are within specific tolerances.
+   * Parametrizes the spline. This method breaks up the spline into various arcs until their dx, dy,
+   * and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
    * @return A list of poses and curvatures that represents various points on the spline.
@@ -84,8 +84,8 @@ public final class SplineParameterizer {
   }
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
-   * dy, and dtheta are within specific tolerances.
+   * Parametrizes the spline. This method breaks up the spline into various arcs until their dx, dy,
+   * and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
    * @param t0 Starting internal spline parameter. It is recommended to use 0.0.

wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java

@@ -238,7 +238,11 @@ public final class NumericalIntegration {
                     .times(h))
                 .normF();
 
-        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        if (truncationError == 0.0) {
+          h = dtSeconds - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
       } while (truncationError > maxError);
 
       dtElapsed += h;

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

@@ -5,6 +5,7 @@
 #include "frc/controller/LTVDifferentialDriveController.h"
 
 #include <cmath>
+#include <stdexcept>
 
 #include "frc/MathUtil.h"
 #include "frc/StateSpaceUtil.h"
@@ -65,6 +66,11 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
   units::meters_per_second_t maxV{
       -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
 
+  if (maxV <= 0_mps) {
+    throw std::domain_error(
+        "Max velocity of plant with 12 V input must be greater than zero.");
+  }
+
   for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.

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

@@ -4,6 +4,8 @@
 
 #include "frc/controller/LTVUnicycleController.h"
 
+#include <stdexcept>
+
 #include "frc/StateSpaceUtil.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "units/math.h"
@@ -37,6 +39,10 @@ LTVUnicycleController::LTVUnicycleController(
 LTVUnicycleController::LTVUnicycleController(
     const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
     units::second_t dt, units::meters_per_second_t maxVelocity) {
+  if (maxVelocity <= 0_mps) {
+    throw std::domain_error("Max velocity must be greater than zero.");
+  }
+
   // The change in global pose for a unicycle is defined by the following three
   // equations.
   //

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

@@ -82,7 +82,7 @@ Pose3d Pose3d::Exp(const Twist3d& twist) const {
   double A;
   double B;
   double C;
-  if (std::abs(theta) < 1E-9) {
+  if (std::abs(theta) < 1E-7) {
     // Taylor Expansions around θ = 0
     // A = 1/1! - θ²/3! + θ⁴/5!
     // B = 1/2! - θ²/4! + θ⁴/6!
@@ -134,7 +134,7 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
   double thetaSq = theta * theta;
 
   double C;
-  if (std::abs(theta) < 1E-9) {
+  if (std::abs(theta) < 1E-7) {
     // Taylor Expansions around θ = 0
     // A = 1/1! - θ²/3! + θ⁴/5!
     // B = 1/2! - θ²/4! + θ⁴/6!

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -138,7 +138,8 @@ Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
  * Creates a cost matrix from the given vector for use with LQR.
  *
  * The cost matrix is constructed using Bryson's rule. The inverse square of
- * each element in the input is taken and placed on the cost matrix diagonal.
+ * each element in the input is placed on the cost matrix diagonal. If a
+ * tolerance is infinity, its cost matrix entry is set to zero.
  *
  * @param costs An array. For a Q matrix, its elements are the maximum allowed
  *              excursions of the states from the reference. For an R matrix,
@@ -151,7 +152,11 @@ Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
   Eigen::DiagonalMatrix<double, N> result;
   auto& diag = result.diagonal();
   for (size_t i = 0; i < N; ++i) {
-    diag(i) = 1.0 / std::pow(costs[i], 2);
+    if (costs[i] == std::numeric_limits<double>::infinity()) {
+      diag(i) = 0.0;
+    } else {
+      diag(i) = 1.0 / std::pow(costs[i], 2);
+    }
   }
   return result;
 }

wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h

@@ -41,6 +41,7 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
    * @param Qelems     The maximum desired error tolerance for each state.
    * @param Relems     The maximum desired control effort for each input.
    * @param dt         Discretization timestep.
+   * @throws std::domain_error if max velocity of plant with 12 V input <= 0.
    */
   LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
                                  units::meter_t trackwidth,

wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h

@@ -36,6 +36,7 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
    * @param dt Discretization timestep.
    * @param maxVelocity The maximum velocity for the controller gain lookup
    *                    table.
+   * @throws std::domain_error if maxVelocity <= 0.
    */
   explicit LTVUnicycleController(
       units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
@@ -48,6 +49,7 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
    * @param dt     Discretization timestep.
    * @param maxVelocity The maximum velocity for the controller gain lookup
    *                    table.
+   * @throws std::domain_error if maxVelocity <= 0.
    */
   LTVUnicycleController(const wpi::array<double, 3>& Qelems,
                         const wpi::array<double, 2>& Relems, units::second_t dt,

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

@@ -314,7 +314,7 @@ class SwerveDrivePoseEstimator {
     // The current gyroscope angle.
     Rotation2d gyroAngle;
 
-    // The distances traveled and rotations meaured at each module.
+    // The distances traveled and rotations measured at each module.
     wpi::array<SwerveModulePosition, NumModules> modulePositions;
 
     /**

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

@@ -14,7 +14,7 @@ namespace frc {
 /**
  * A change in distance along a 2D arc since the last pose update. We can use
  * ideas from differential calculus to create new Pose2ds from a Twist2d and
- * vise versa.
+ * vice versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */