Merge branch 'main' into update-color

ntcoreffi/src/main/native/cpp/DataLogManager.cpp

@@ -217,12 +217,12 @@ static std::string MakeLogDir(std::string_view dir) {
   }
 #ifdef __FRC_ROBORIO__
   // prefer a mounted USB drive if one is accessible
-  constexpr std::string_view usbDir{"/u"};
   std::error_code ec;
-  auto s = fs::status(usbDir, ec);
+  auto s = fs::status("/u", ec);
   if (!ec && fs::is_directory(s) &&
       (s.permissions() & fs::perms::others_write) != fs::perms::none) {
-    return std::string{usbDir};
+    fs::create_directory("/u/logs", ec);
+    return "/u/logs";
   }
   if (RobotBase::GetRuntimeType() == kRoboRIO) {
     FRC_ReportError(warn::Warning,

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

@@ -261,7 +261,7 @@ public void run() {
       if (RobotBase.isSimulation()) {
         subsystem.simulationPeriodic();
       }
-      m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
+      m_watchdog.addEpoch(subsystem.getName() + ".periodic()");
     }
 
     // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from

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

@@ -40,6 +40,15 @@ default void periodic() {}
    */
   default void simulationPeriodic() {}
 
+  /**
+   * Gets the subsystem name of this Subsystem.
+   *
+   * @return Subsystem name
+   */
+  default String getName() {
+    return this.getClass().getSimpleName();
+  }
+
   /**
    * Sets the default {@link Command} of the subsystem. The default command will be automatically
    * scheduled when no other commands are scheduled that require the subsystem. Default commands

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

@@ -28,6 +28,7 @@ public SubsystemBase() {
    *
    * @return Name
    */
+  @Override
   public String getName() {
     return SendableRegistry.getName(this);
   }

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

@@ -183,7 +183,7 @@ void CommandScheduler::Run() {
     if constexpr (frc::RobotBase::IsSimulation()) {
       subsystem.getFirst()->SimulationPeriodic();
     }
-    m_watchdog.AddEpoch("Subsystem Periodic()");
+    m_watchdog.AddEpoch(subsystem.getFirst()->GetName() + ".Periodic()");
   }
 
   // Cache the active instance to avoid concurrency problems if SetActiveLoop()

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/Subsystem.h"
 
+#include <wpi/Demangle.h>
+
 #include "frc2/command/CommandPtr.h"
 #include "frc2/command/Commands.h"
 
@@ -16,6 +18,10 @@ void Subsystem::Periodic() {}
 
 void Subsystem::SimulationPeriodic() {}
 
+std::string Subsystem::GetName() const {
+  return wpi::GetTypeName(*this);
+}
+
 void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
   CommandScheduler::GetInstance().SetDefaultCommand(this,
                                                     std::move(defaultCommand));

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

@@ -18,11 +18,6 @@
 
 namespace frc2 {
 
-template <typename T>
-std::string GetTypeName(const T& type) {
-  return wpi::Demangle(typeid(type).name());
-}
-
 /**
  * A state machine representing a complete action to be performed by the robot.
  * Commands are run by the CommandScheduler, and can be composed into

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

@@ -6,6 +6,7 @@
 
 #include <concepts>
 #include <functional>
+#include <string>
 #include <utility>
 
 #include <wpi/FunctionExtras.h>
@@ -59,6 +60,13 @@ class Subsystem {
    */
   virtual void SimulationPeriodic();
 
+  /**
+   * Gets the name of this Subsystem.
+   *
+   * @return Name
+   */
+  virtual std::string GetName() const;
+
   /**
    * Sets the default Command of the subsystem.  The default command will be
    * automatically scheduled when no other commands are scheduled that require

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

@@ -30,7 +30,7 @@ class SubsystemBase : public Subsystem,
    *
    * @return Name
    */
-  std::string GetName() const;
+  std::string GetName() const override;
 
   /**
    * Sets the name of this Subsystem.

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

@@ -64,12 +64,12 @@ static std::string MakeLogDir(std::string_view dir) {
   }
 #ifdef __FRC_ROBORIO__
   // prefer a mounted USB drive if one is accessible
-  constexpr std::string_view usbDir{"/u"};
   std::error_code ec;
-  auto s = fs::status(usbDir, ec);
+  auto s = fs::status("/u", ec);
   if (!ec && fs::is_directory(s) &&
       (s.permissions() & fs::perms::others_write) != fs::perms::none) {
-    return std::string{usbDir};
+    fs::create_directory("/u/logs", ec);
+    return "/u/logs";
   }
   if (RobotBase::GetRuntimeType() == kRoboRIO) {
     FRC_ReportError(warn::Warning,

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

@@ -214,7 +214,10 @@ private static String makeLogDir(String dir) {
         // prefer a mounted USB drive if one is accessible
         Path usbDir = Paths.get("/u").toRealPath();
         if (Files.isWritable(usbDir)) {
-          return usbDir.toString();
+          if (!new File("/u/logs").mkdir()) {
+            // ignored
+          }
+          return "/u/logs";
         }
       } catch (IOException ex) {
         // ignored

wpimath/src/main/java/edu/wpi/first/math/optimization/SimulatedAnnealing.java

@@ -0,0 +1,79 @@
+// 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.math.optimization;
+
+import java.util.function.Function;
+import java.util.function.ToDoubleFunction;
+
+/**
+ * An implementation of the Simulated Annealing stochastic nonlinear optimization method.
+ *
+ * @see <a
+ *     href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
+ * @param <State> The type of the state to optimize.
+ */
+public final class SimulatedAnnealing<State> {
+  private final double m_initialTemperature;
+  private final Function<State, State> m_neighbor;
+  private final ToDoubleFunction<State> m_cost;
+
+  /**
+   * Constructor for Simulated Annealing that can be used for the same functions but with different
+   * initial states.
+   *
+   * @param initialTemperature The initial temperature. Higher temperatures make it more likely a
+   *     worse state will be accepted during iteration, helping to avoid local minima. The
+   *     temperature is decreased over time.
+   * @param neighbor Function that generates a random neighbor of the current state.
+   * @param cost Function that returns the scalar cost of a state.
+   */
+  public SimulatedAnnealing(
+      double initialTemperature, Function<State, State> neighbor, ToDoubleFunction<State> cost) {
+    m_initialTemperature = initialTemperature;
+    m_neighbor = neighbor;
+    m_cost = cost;
+  }
+
+  /**
+   * Runs the Simulated Annealing algorithm.
+   *
+   * @param initialGuess The initial state.
+   * @param iterations Number of iterations to run the solver.
+   * @return The optimized stater.
+   */
+  public State solve(State initialGuess, int iterations) {
+    State minState = initialGuess;
+    double minCost = Double.MAX_VALUE;
+
+    State state = initialGuess;
+    double cost = m_cost.applyAsDouble(state);
+
+    for (int i = 0; i < iterations; ++i) {
+      double temperature = m_initialTemperature / i;
+
+      State proposedState = m_neighbor.apply(state);
+      double proposedCost = m_cost.applyAsDouble(proposedState);
+      double deltaCost = proposedCost - cost;
+
+      double acceptanceProbability = Math.exp(-deltaCost / temperature);
+
+      // If cost went down or random number exceeded acceptance probability,
+      // accept the proposed state
+      if (deltaCost < 0 || acceptanceProbability >= Math.random()) {
+        state = proposedState;
+        cost = proposedCost;
+      }
+
+      // If proposed cost is less than minimum, the proposed state becomes the
+      // new minimum
+      if (proposedCost < minCost) {
+        minState = proposedState;
+        minCost = proposedCost;
+      }
+    }
+
+    return minState;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java

@@ -0,0 +1,108 @@
+// 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.math.path;
+
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.optimization.SimulatedAnnealing;
+import java.util.function.ToDoubleBiFunction;
+
+/**
+ * Given a list of poses, this class finds the shortest possible route that visits each pose exactly
+ * once and returns to the origin pose.
+ *
+ * @see <a
+ *     href="https://en.wikipedia.org/wiki/Travelling_salesman_problem">https://en.wikipedia.org/wiki/Travelling_salesman_problem</a>
+ */
+public class TravelingSalesman {
+  // Default cost is 2D distance between poses
+  private final ToDoubleBiFunction<Pose2d, Pose2d> m_cost;
+
+  /**
+   * Constructs a traveling salesman problem solver with a cost function defined as the 2D distance
+   * between poses.
+   */
+  public TravelingSalesman() {
+    this((Pose2d a, Pose2d b) -> Math.hypot(a.getX() - b.getX(), a.getY() - b.getY()));
+  }
+
+  /**
+   * Constructs a traveling salesman problem solver with a user-provided cost function.
+   *
+   * @param cost Function that returns the cost between two poses. The sum of the costs for every
+   *     pair of poses is minimized.
+   */
+  public TravelingSalesman(ToDoubleBiFunction<Pose2d, Pose2d> cost) {
+    m_cost = cost;
+  }
+
+  /**
+   * Finds the path through every pose that minimizes the cost.
+   *
+   * @param <Poses> A Num defining the length of the path and the number of poses.
+   * @param poses An array of Pose2ds the path must pass through.
+   * @param iterations The number of times the solver attempts to find a better random neighbor.
+   * @return The optimized path as an array of Pose2ds.
+   */
+  public <Poses extends Num> Pose2d[] solve(Pose2d[] poses, int iterations) {
+    var solver =
+        new SimulatedAnnealing<>(
+            1.0,
+            this::neighbor,
+            // Total cost is sum of all costs between adjacent pose pairs in path
+            (Vector<Poses> state) -> {
+              double sum = 0.0;
+              for (int i = 0; i < state.getNumRows(); ++i) {
+                sum +=
+                    m_cost.applyAsDouble(
+                        poses[(int) state.get(i, 0)],
+                        poses[(int) (state.get((i + 1) % poses.length, 0))]);
+              }
+              return sum;
+            });
+
+    var initial = new Vector<Poses>(() -> poses.length);
+    for (int i = 0; i < poses.length; ++i) {
+      initial.set(i, 0, i);
+    }
+
+    var indices = solver.solve(initial, iterations);
+
+    var solution = new Pose2d[poses.length];
+    for (int i = 0; i < poses.length; ++i) {
+      solution[i] = poses[(int) indices.get(i, 0)];
+    }
+
+    return solution;
+  }
+
+  /**
+   * A random neighbor is generated to try to replace the current one.
+   *
+   * @param state A vector that is a list of indices that defines the path through the path array.
+   * @return Generates a random neighbor of the current state by flipping a random range in the path
+   *     array.
+   */
+  private <Poses extends Num> Vector<Poses> neighbor(Vector<Poses> state) {
+    var proposedState = new Vector<Poses>(state);
+
+    int rangeStart = (int) (Math.random() * (state.getNumRows() - 1));
+    int rangeEnd = (int) (Math.random() * (state.getNumRows() - 1));
+    if (rangeEnd < rangeStart) {
+      int temp = rangeEnd;
+      rangeEnd = rangeStart;
+      rangeStart = temp;
+    }
+
+    for (int i = rangeStart; i <= (rangeStart + rangeEnd) / 2; ++i) {
+      double temp = proposedState.get(i, 0);
+      proposedState.set(i, 0, state.get(rangeEnd - (i - rangeStart), 0));
+      proposedState.set(rangeEnd - (i - rangeStart), 0, temp);
+    }
+
+    return proposedState;
+  }
+}

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -64,28 +64,25 @@ class TimeInterpolatableBuffer {
    * @param sample The sample object.
    */
   void AddSample(units::second_t time, T sample) {
-    // Add the new state into the vector.
+    // Add the new state into the vector
     if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
       m_pastSnapshots.emplace_back(time, sample);
     } else {
       auto first_after = std::upper_bound(
           m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
           [](auto t, const auto& pair) { return t < pair.first; });
 
-      // Don't access this before ensuring first_after isn't first.
-      auto last_not_greater_than = first_after - 1;
-
-      if (first_after == m_pastSnapshots.begin() ||
-          last_not_greater_than == m_pastSnapshots.begin() ||
-          last_not_greater_than->first < time) {
-        // Two cases handled together:
-        // 1. All entries come after the sample
-        // 2. Some entries come before the sample, but none are recorded with
-        // the same time
-        m_pastSnapshots.insert(first_after, std::pair(time, sample));
+      if (first_after == m_pastSnapshots.begin()) {
+        // All entries come after the sample
+        m_pastSnapshots.insert(first_after, std::pair{time, sample});
+      } else if (auto last_not_greater_than = first_after - 1;
+                 last_not_greater_than == m_pastSnapshots.begin() ||
+                 last_not_greater_than->first < time) {
+        // Some entries come before the sample, but none are recorded with the
+        // same time
+        m_pastSnapshots.insert(first_after, std::pair{time, sample});
       } else {
-        // Final case:
-        // 3. An entry exists with the same recorded time.
+        // An entry exists with the same recorded time
         last_not_greater_than->second = sample;
       }
     }