[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>

wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java

@@ -0,0 +1,215 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.sysid;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Current;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.util.datalog.StringLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Utility for logging data from a SysId test routine. Each complete routine (quasistatic and
+ * dynamic, forward and reverse) should have its own SysIdRoutineLog instance, with a unique log
+ * name.
+ */
+public class SysIdRoutineLog {
+  private final Map<String, Map<String, DoubleLogEntry>> m_logEntries = new HashMap<>();
+  private final String m_logName;
+  private final StringLogEntry m_state;
+
+  /**
+   * Create a new logging utility for a SysId test routine.
+   *
+   * @param logName The name for the test routine in the log. Should be unique between complete test
+   *     routines (quasistatic and dynamic, forward and reverse). The current state of this test
+   *     (e.g. "quasistatic-forward") will appear in WPILog under the "sysid-test-state-logName"
+   *     entry.
+   */
+  public SysIdRoutineLog(String logName) {
+    m_logName = logName;
+    m_state = new StringLogEntry(DataLogManager.getLog(), "sysid-test-state-" + logName);
+    m_state.append(State.kNone.toString());
+  }
+
+  /** Possible state of a SysId routine. */
+  public enum State {
+    kQuasistaticForward("quasistatic-forward"),
+    kQuasistaticReverse("quasistatic-reverse"),
+    kDynamicForward("dynamic-forward"),
+    kDynamicReverse("dynamic-reverse"),
+    kNone("none");
+
+    private final String m_state;
+
+    State(String state) {
+      m_state = state;
+    }
+
+    @Override
+    public String toString() {
+      return m_state;
+    }
+  }
+
+  /** Allows logging of data for a single motor during a SysIdRoutine. */
+  public class MotorLog {
+    private final String m_motorName;
+
+    /**
+     * Create a new SysId motor log handle.
+     *
+     * @param motorName The name of the motor whose data is being logged.
+     */
+    private MotorLog(String motorName) {
+      m_motorName = motorName;
+      m_logEntries.put(motorName, new HashMap<>());
+    }
+
+    /**
+     * Record a generic data value from this motor.
+     *
+     * @param name The name of the data field being recorded.
+     * @param value The numeric value of the data field.
+     * @param unit The unit string of the data field.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog value(String name, double value, String unit) {
+      var motorEntries = m_logEntries.get(m_motorName);
+      var entry = motorEntries.get(name);
+
+      if (entry == null) {
+        var log = DataLogManager.getLog();
+
+        entry = new DoubleLogEntry(log, name + "-" + m_motorName + "-" + m_logName, unit);
+        motorEntries.put(name, entry);
+      }
+
+      entry.append(value);
+      return this;
+    }
+
+    /**
+     * Record a voltage value from this motor.
+     *
+     * @param voltage The voltage to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog voltage(Measure<Voltage> voltage) {
+      return value("voltage", voltage.in(Volts), Volts.name());
+    }
+
+    /**
+     * Record a linear position value from this motor.
+     *
+     * @param position The linear position to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearPosition(Measure<Distance> position) {
+      return value("position", position.in(Meters), Meters.name());
+    }
+
+    /**
+     * Record an angular position value from this motor.
+     *
+     * @param position The angular position to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularPosition(Measure<Angle> position) {
+      return value("position", position.in(Rotations), Rotations.name());
+    }
+
+    /**
+     * Record a linear velocity value from this motor.
+     *
+     * @param velocity The linear velocity to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearVelocity(Measure<Velocity<Distance>> velocity) {
+      return value("velocity", velocity.in(MetersPerSecond), MetersPerSecond.name());
+    }
+
+    /**
+     * Record an angular velocity value from this motor.
+     *
+     * @param velocity The angular velocity to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularVelocity(Measure<Velocity<Angle>> velocity) {
+      return value("velocity", velocity.in(RotationsPerSecond), RotationsPerSecond.name());
+    }
+
+    /**
+     * Record a linear acceleration value from this motor.
+     *
+     * @param acceleration The linear acceleration to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearAcceleration(Measure<Velocity<Velocity<Distance>>> acceleration) {
+      return value(
+          "position",
+          acceleration.in(MetersPerSecond.per(Second)),
+          MetersPerSecond.per(Second).name());
+    }
+
+    /**
+     * Record an angular acceleration value from this motor.
+     *
+     * @param acceleration The angular acceleration to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularAcceleration(Measure<Velocity<Velocity<Angle>>> acceleration) {
+      return value(
+          "position",
+          acceleration.in(RotationsPerSecond.per(Second)),
+          RotationsPerSecond.per(Second).name());
+    }
+
+    /**
+     * Record a current value for this motor.
+     *
+     * @param current The current to record.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog current(Measure<Current> current) {
+      value("current", current.in(Amps), Amps.name());
+      return this;
+    }
+  }
+
+  /**
+   * Returns the logging handle for a given motor.
+   *
+   * @param motorName The name of the motor to log data from.
+   * @return The logging handle for the specified motor.
+   */
+  public MotorLog motor(String motorName) {
+    return new MotorLog(motorName);
+  }
+
+  /**
+   * Records the current state of the SysId test routine. Should be called once per iteration during
+   * tests with the type of the current test, and once upon test end with state `none`.
+   *
+   * @param state The current state of the SysId test routine.
+   */
+  public void recordState(State state) {
+    m_state.append(state.toString());
+  }
+}