001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.sysid;
006
007import static edu.wpi.first.units.Units.Amps;
008import static edu.wpi.first.units.Units.Meters;
009import static edu.wpi.first.units.Units.MetersPerSecond;
010import static edu.wpi.first.units.Units.Rotations;
011import static edu.wpi.first.units.Units.RotationsPerSecond;
012import static edu.wpi.first.units.Units.Second;
013import static edu.wpi.first.units.Units.Volts;
014
015import edu.wpi.first.units.Angle;
016import edu.wpi.first.units.Current;
017import edu.wpi.first.units.Distance;
018import edu.wpi.first.units.Measure;
019import edu.wpi.first.units.Velocity;
020import edu.wpi.first.units.Voltage;
021import edu.wpi.first.util.datalog.DoubleLogEntry;
022import edu.wpi.first.util.datalog.StringLogEntry;
023import edu.wpi.first.wpilibj.DataLogManager;
024import java.util.HashMap;
025import java.util.Map;
026
027/**
028 * Utility for logging data from a SysId test routine. Each complete routine (quasistatic and
029 * dynamic, forward and reverse) should have its own SysIdRoutineLog instance, with a unique log
030 * name.
031 */
032public class SysIdRoutineLog {
033  private final Map<String, Map<String, DoubleLogEntry>> m_logEntries = new HashMap<>();
034  private final String m_logName;
035  private StringLogEntry m_state;
036
037  /**
038   * Create a new logging utility for a SysId test routine.
039   *
040   * @param logName The name for the test routine in the log. Should be unique between complete test
041   *     routines (quasistatic and dynamic, forward and reverse). The current state of this test
042   *     (e.g. "quasistatic-forward") will appear in WPILog under the "sysid-test-state-logName"
043   *     entry.
044   */
045  public SysIdRoutineLog(String logName) {
046    m_logName = logName;
047  }
048
049  /** Possible state of a SysId routine. */
050  public enum State {
051    /** Quasistatic forward test. */
052    kQuasistaticForward("quasistatic-forward"),
053    /** Quasistatic reverse test. */
054    kQuasistaticReverse("quasistatic-reverse"),
055    /** Dynamic forward test. */
056    kDynamicForward("dynamic-forward"),
057    /** Dynamic reverse test. */
058    kDynamicReverse("dynamic-reverse"),
059    /** No test. */
060    kNone("none");
061
062    private final String m_state;
063
064    State(String state) {
065      m_state = state;
066    }
067
068    @Override
069    public String toString() {
070      return m_state;
071    }
072  }
073
074  /** Logs data from a single motor during a SysIdRoutine. */
075  public class MotorLog {
076    private final String m_motorName;
077
078    /**
079     * Create a new SysId motor log handle.
080     *
081     * @param motorName The name of the motor whose data is being logged.
082     */
083    private MotorLog(String motorName) {
084      m_motorName = motorName;
085      m_logEntries.put(motorName, new HashMap<>());
086    }
087
088    /**
089     * Log a generic data value from the motor.
090     *
091     * @param name The name of the data field being recorded.
092     * @param value The numeric value of the data field.
093     * @param unit The unit string of the data field.
094     * @return The motor log (for call chaining).
095     */
096    public MotorLog value(String name, double value, String unit) {
097      var motorEntries = m_logEntries.get(m_motorName);
098      var entry = motorEntries.get(name);
099
100      if (entry == null) {
101        var log = DataLogManager.getLog();
102
103        entry = new DoubleLogEntry(log, name + "-" + m_motorName + "-" + m_logName, unit);
104        motorEntries.put(name, entry);
105      }
106
107      entry.append(value);
108      return this;
109    }
110
111    /**
112     * Log the voltage applied to the motor.
113     *
114     * @param voltage The voltage to record.
115     * @return The motor log (for call chaining).
116     */
117    public MotorLog voltage(Measure<Voltage> voltage) {
118      return value("voltage", voltage.in(Volts), Volts.name());
119    }
120
121    /**
122     * Log the linear position of the motor.
123     *
124     * @param position The linear position to record.
125     * @return The motor log (for call chaining).
126     */
127    public MotorLog linearPosition(Measure<Distance> position) {
128      return value("position", position.in(Meters), Meters.name());
129    }
130
131    /**
132     * Log the angular position of the motor.
133     *
134     * @param position The angular position to record.
135     * @return The motor log (for call chaining).
136     */
137    public MotorLog angularPosition(Measure<Angle> position) {
138      return value("position", position.in(Rotations), Rotations.name());
139    }
140
141    /**
142     * Log the linear velocity of the motor.
143     *
144     * @param velocity The linear velocity to record.
145     * @return The motor log (for call chaining).
146     */
147    public MotorLog linearVelocity(Measure<Velocity<Distance>> velocity) {
148      return value("velocity", velocity.in(MetersPerSecond), MetersPerSecond.name());
149    }
150
151    /**
152     * Log the angular velocity of the motor.
153     *
154     * @param velocity The angular velocity to record.
155     * @return The motor log (for call chaining).
156     */
157    public MotorLog angularVelocity(Measure<Velocity<Angle>> velocity) {
158      return value("velocity", velocity.in(RotationsPerSecond), RotationsPerSecond.name());
159    }
160
161    /**
162     * Log the linear acceleration of the motor.
163     *
164     * <p>This is optional; SysId can perform an accurate fit without it.
165     *
166     * @param acceleration The linear acceleration to record.
167     * @return The motor log (for call chaining).
168     */
169    public MotorLog linearAcceleration(Measure<Velocity<Velocity<Distance>>> acceleration) {
170      return value(
171          "acceleration",
172          acceleration.in(MetersPerSecond.per(Second)),
173          MetersPerSecond.per(Second).name());
174    }
175
176    /**
177     * Log the angular acceleration of the motor.
178     *
179     * <p>This is optional; SysId can perform an accurate fit without it.
180     *
181     * @param acceleration The angular acceleration to record.
182     * @return The motor log (for call chaining).
183     */
184    public MotorLog angularAcceleration(Measure<Velocity<Velocity<Angle>>> acceleration) {
185      return value(
186          "acceleration",
187          acceleration.in(RotationsPerSecond.per(Second)),
188          RotationsPerSecond.per(Second).name());
189    }
190
191    /**
192     * Log the current applied to the motor.
193     *
194     * <p>This is optional; SysId can perform an accurate fit without it.
195     *
196     * @param current The current to record.
197     * @return The motor log (for call chaining).
198     */
199    public MotorLog current(Measure<Current> current) {
200      value("current", current.in(Amps), Amps.name());
201      return this;
202    }
203  }
204
205  /**
206   * Log data from a motor during a SysId routine.
207   *
208   * @param motorName The name of the motor.
209   * @return Handle with chainable callbacks to log individual data fields.
210   */
211  public MotorLog motor(String motorName) {
212    return new MotorLog(motorName);
213  }
214
215  /**
216   * Records the current state of the SysId test routine. Should be called once per iteration during
217   * tests with the type of the current test, and once upon test end with state `none`.
218   *
219   * @param state The current state of the SysId test routine.
220   */
221  public void recordState(State state) {
222    if (m_state == null) {
223      m_state = new StringLogEntry(DataLogManager.getLog(), "sysid-test-state-" + m_logName);
224    }
225    m_state.append(state.toString());
226  }
227}