[wpilib] Add SysIdRoutine logging utility and command factory (#6033)

Co-authored-by: Drew Williams <williams.r.drew@gmail.com>
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java

@@ -0,0 +1,264 @@
+// 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.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+import static java.util.Map.entry;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Time;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.Map;
+import java.util.function.Consumer;
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors.
+ *
+ * <p>A single subsystem may have multiple mechanisms, but mechanisms should not share test
+ * routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the
+ * log name of the recorded data is determined by the mechanism name.
+ *
+ * <p>The test state (e.g. "quasistatic-forward") is logged once per iteration during test
+ * execution, and once with state "none" when a test ends. Motor frames are logged every iteration
+ * during test execution.
+ *
+ * <p>Timestamps are not coordinated across data, so motor frames and test state tags may be
+ * recorded on different log frames. Because frame alignment is not guaranteed, SysId parses the log
+ * by using the test state flag to determine the timestamp range for each section of the test, and
+ * then extracts the motor frames within the valid timestamp ranges. If a given test was run
+ * multiple times in a single logfile, the user will need to select which of the tests to use for
+ * the fit in the analysis tool.
+ */
+public class SysIdRoutine extends SysIdRoutineLog {
+  private final Config m_config;
+  private final Mechanism m_mechanism;
+  private final MutableMeasure<Voltage> m_outputVolts = mutable(Volts.of(0));
+  private final Consumer<State> m_recordState;
+
+  /**
+   * Create a new SysId characterization routine.
+   *
+   * @param config Hardware-independent parameters for the SysId routine.
+   * @param mechanism Hardware interface for the SysId routine.
+   */
+  public SysIdRoutine(Config config, Mechanism mechanism) {
+    super(mechanism.m_subsystem.getName());
+    m_config = config;
+    m_mechanism = mechanism;
+    m_recordState = config.m_recordState != null ? config.m_recordState : this::recordState;
+  }
+
+  /** Hardware-independent configuration for a SysId test routine. */
+  public static class Config {
+    public final Measure<Velocity<Voltage>> m_rampRate;
+    public final Measure<Voltage> m_stepVoltage;
+    public final Measure<Time> m_timeout;
+    public final Consumer<State> m_recordState;
+
+    /**
+     * Create a new configuration for a SysId test routine.
+     *
+     * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+     *     per second if left null.
+     * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+     *     volts if left null.
+     * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+     *     null.
+     * @param recordState Optional handle for recording test state in a third-party logging
+     *     solution. If provided, the test routine state will be passed to this callback instead of
+     *     logged in WPILog.
+     */
+    public Config(
+        Measure<Velocity<Voltage>> rampRate,
+        Measure<Voltage> stepVoltage,
+        Measure<Time> timeout,
+        Consumer<State> recordState) {
+      m_rampRate = rampRate != null ? rampRate : Volts.of(1).per(Seconds.of(1));
+      m_stepVoltage = stepVoltage != null ? stepVoltage : Volts.of(7);
+      m_timeout = timeout != null ? timeout : Seconds.of(10);
+      m_recordState = recordState;
+    }
+
+    /**
+     * Create a new configuration for a SysId test routine.
+     *
+     * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+     *     per second if left null.
+     * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+     *     volts if left null.
+     * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+     *     null.
+     */
+    public Config(
+        Measure<Velocity<Voltage>> rampRate, Measure<Voltage> stepVoltage, Measure<Time> timeout) {
+      this(rampRate, stepVoltage, timeout, null);
+    }
+
+    /**
+     * Create a default configuration for a SysId test routine with all default settings.
+     *
+     * <p>rampRate: 1 volt/sec
+     *
+     * <p>stepVoltage: 7 volts
+     *
+     * <p>timeout: 10 seconds
+     */
+    public Config() {
+      this(null, null, null, null);
+    }
+  }
+
+  /**
+   * A mechanism to be characterized by a SysId routine. Defines callbacks needed for the SysId test
+   * routine to control and record data from the mechanism.
+   */
+  public static class Mechanism {
+    public final Consumer<Measure<Voltage>> m_drive;
+    public final Consumer<SysIdRoutineLog> m_log;
+    public final Subsystem m_subsystem;
+    public final String m_name;
+
+    /**
+     * Create a new mechanism specification for a SysId routine.
+     *
+     * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+     *     routines.
+     * @param log Returns measured data (voltages, positions, velocities) of the mechanism motors
+     *     during test routines. To return data, call `recordFrame` on the supplied
+     *     `SysIdRoutineLog` instance. Multiple motors can return data within a single `log`
+     *     callback by calling `recordFrame` multiple times.
+     * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+     *     Will be declared as a requirement for the returned test commands.
+     * @param name The name of the mechanism being tested. Will be appended to the log entry title
+     *     for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of
+     *     the subsystem if left null.
+     */
+    public Mechanism(
+        Consumer<Measure<Voltage>> drive,
+        Consumer<SysIdRoutineLog> log,
+        Subsystem subsystem,
+        String name) {
+      m_drive = drive;
+      m_log = log != null ? log : l -> {};
+      m_subsystem = subsystem;
+      m_name = name != null ? name : subsystem.getName();
+    }
+
+    /**
+     * Create a new mechanism specification for a SysId routine. Defaults the mechanism name to the
+     * subsystem name.
+     *
+     * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+     *     routines.
+     * @param log Returns measured data (voltages, positions, velocities) of the mechanism motors
+     *     during test routines. To return data, call `recordFrame` on the supplied
+     *     `SysIdRoutineLog` instance. Multiple motors can return data within a single `log`
+     *     callback by calling `recordFrame` multiple times.
+     * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+     *     Will be declared as a requirement for the returned test commands. The subsystem's `name`
+     *     will be appended to the log entry title for the routine's test state, e.g.
+     *     "sysid-test-state-subsystem".
+     */
+    public Mechanism(
+        Consumer<Measure<Voltage>> drive, Consumer<SysIdRoutineLog> log, Subsystem subsystem) {
+      this(drive, log, subsystem, null);
+    }
+  }
+
+  /** Motor direction for a SysId test. */
+  public enum Direction {
+    kForward,
+    kReverse
+  }
+
+  /**
+   * Returns a command to run a quasistatic test in the specified direction.
+   *
+   * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+   * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+   * 0 volts.
+   *
+   * @param direction The direction in which to run the test.
+   * @return A command to run the test.
+   */
+  public Command quasistatic(Direction direction) {
+    Timer timer = new Timer();
+    double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+    State state =
+        Map.ofEntries(
+                entry(Direction.kForward, State.kQuasistaticForward),
+                entry(Direction.kReverse, State.kQuasistaticReverse))
+            .get(direction);
+
+    return m_mechanism
+        .m_subsystem
+        .runOnce(timer::start)
+        .andThen(
+            m_mechanism.m_subsystem.run(
+                () -> {
+                  m_mechanism.m_drive.accept(
+                      m_outputVolts.mut_replace(
+                          outputSign * timer.get() * m_config.m_rampRate.in(Volts.per(Second)),
+                          Volts));
+                  m_mechanism.m_log.accept(this);
+                  m_recordState.accept(state);
+                }))
+        .finallyDo(
+            () -> {
+              m_mechanism.m_drive.accept(Volts.of(0));
+              m_recordState.accept(State.kNone);
+              timer.stop();
+            })
+        .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+        .withTimeout(m_config.m_timeout.in(Seconds));
+  }
+
+  /**
+   * Returns a command to run a dynamic test in the specified direction.
+   *
+   * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+   * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+   * 0 volts.
+   *
+   * @param direction The direction in which to run the test.
+   * @return A command to run the test.
+   */
+  public Command dynamic(Direction direction) {
+    double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+    State state =
+        Map.ofEntries(
+                entry(Direction.kForward, State.kDynamicForward),
+                entry(Direction.kReverse, State.kDynamicReverse))
+            .get(direction);
+
+    return m_mechanism
+        .m_subsystem
+        .runOnce(
+            () -> m_outputVolts.mut_replace(m_config.m_stepVoltage.in(Volts) * outputSign, Volts))
+        .andThen(
+            m_mechanism.m_subsystem.run(
+                () -> {
+                  m_mechanism.m_drive.accept(m_outputVolts);
+                  m_mechanism.m_log.accept(this);
+                  m_recordState.accept(state);
+                }))
+        .finallyDo(
+            () -> {
+              m_mechanism.m_drive.accept(Volts.of(0));
+              m_recordState.accept(State.kNone);
+            })
+        .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+        .withTimeout(m_config.m_timeout.in(Seconds));
+  }
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/sysid/SysIdRoutine.h"
+
+using namespace frc2::sysid;
+
+frc2::CommandPtr SysIdRoutine::Quasistatic(Direction direction) {
+  std::unordered_map<Direction, frc::sysid::State> stateOptions{
+      {Direction::kForward, frc::sysid::State::kQuasistaticForward},
+      {Direction::kReverse, frc::sysid::State::kQuasistaticReverse},
+  };
+  frc::sysid::State state = stateOptions[direction];
+  double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+  return m_mechanism.m_subsystem
+      ->RunOnce([this] {
+        timer.Reset();
+        timer.Start();
+      })
+      .AndThen(
+          m_mechanism.m_subsystem
+              ->Run([this, state, outputSign] {
+                m_outputVolts = outputSign * timer.Get() * m_config.m_rampRate;
+                m_mechanism.m_drive(m_outputVolts);
+                m_mechanism.m_log(this);
+                m_recordState(state);
+              })
+              .FinallyDo([this] {
+                m_mechanism.m_drive(0_V);
+                m_recordState(frc::sysid::State::kNone);
+                timer.Stop();
+              })
+              .WithName("sysid-" +
+                        frc::sysid::SysIdRoutineLog::StateEnumToString(state) +
+                        "-" + m_mechanism.m_name)
+              .WithTimeout(m_config.m_timeout));
+}
+
+frc2::CommandPtr SysIdRoutine::Dynamic(Direction direction) {
+  std::unordered_map<Direction, frc::sysid::State> stateOptions{
+      {Direction::kForward, frc::sysid::State::kDynamicForward},
+      {Direction::kReverse, frc::sysid::State::kDynamicReverse},
+  };
+  frc::sysid::State state = stateOptions[direction];
+  double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+  return m_mechanism.m_subsystem
+      ->RunOnce([this] { m_outputVolts = m_config.m_stepVoltage; })
+      .AndThen(m_mechanism.m_subsystem->Run([this, state, outputSign] {
+        m_mechanism.m_drive(m_outputVolts * outputSign);
+        m_mechanism.m_log(this);
+        m_recordState(state);
+      }))
+      .FinallyDo([this] {
+        m_mechanism.m_drive(0_V);
+        m_recordState(frc::sysid::State::kNone);
+      })
+      .WithName("sysid-" +
+                frc::sysid::SysIdRoutineLog::StateEnumToString(state) + "-" +
+                m_mechanism.m_name)
+      .WithTimeout(m_config.m_timeout);
+}