diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java
new file mode 100644
index 00000000000..bac491da422
--- /dev/null
+++ b/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.
+ *
+ *
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.
+ *
+ *
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.
+ *
+ *
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 m_outputVolts = mutable(Volts.of(0));
+ private final Consumer 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> m_rampRate;
+ public final Measure m_stepVoltage;
+ public final Measure m_timeout;
+ public final Consumer 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> rampRate,
+ Measure stepVoltage,
+ Measure timeout,
+ Consumer 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> rampRate, Measure stepVoltage, Measure timeout) {
+ this(rampRate, stepVoltage, timeout, null);
+ }
+
+ /**
+ * Create a default configuration for a SysId test routine with all default settings.
+ *
+ * rampRate: 1 volt/sec
+ *
+ *
stepVoltage: 7 volts
+ *
+ *
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> m_drive;
+ public final Consumer 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> drive,
+ Consumer 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> drive, Consumer 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.
+ *
+ * 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.
+ *
+ *
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));
+ }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp
new file mode 100644
index 00000000000..5f677fe2a93
--- /dev/null
+++ b/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 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 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);
+}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
new file mode 100644
index 00000000000..8e323d0641e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
@@ -0,0 +1,162 @@
+// 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.
+
+#pragma once
+
+#include
+#include
+
+#include
+#include
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Subsystem.h"
+
+namespace frc2::sysid {
+
+using ramp_rate_t = units::unit_t<
+ units::compound_unit>>;
+
+/** Hardware-independent configuration for a SysId test routine. */
+class Config {
+ public:
+ /**
+ * 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.
+ */
+ Config(std::optional rampRate,
+ std::optional stepVoltage,
+ std::optional timeout,
+ std::optional> recordState) {
+ if (rampRate) {
+ m_rampRate = rampRate.value();
+ }
+ if (stepVoltage) {
+ m_stepVoltage = stepVoltage.value();
+ }
+ if (timeout) {
+ m_timeout = timeout.value();
+ }
+ if (recordState) {
+ m_recordState = recordState.value();
+ }
+ }
+ ramp_rate_t m_rampRate{1_V / 1_s};
+ units::volt_t m_stepVoltage{7_V};
+ units::second_t m_timeout{10_s};
+ std::function m_recordState;
+};
+
+class Mechanism {
+ public:
+ /**
+ * 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 `MotorLog` 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.
+ */
+ Mechanism(std::function drive,
+ std::function log,
+ frc2::Subsystem* subsystem, const std::string& name)
+ : m_drive(drive), m_log(log), m_subsystem(subsystem), m_name(name) {}
+
+ /**
+ * 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 `MotorLog` 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".
+ */
+ Mechanism(std::function drive,
+ std::function log,
+ frc2::Subsystem* subsystem)
+ : m_drive(drive),
+ m_log(log),
+ m_subsystem(subsystem),
+ m_name(m_subsystem->GetName()) {}
+ std::function m_drive;
+ std::function m_log;
+ frc2::Subsystem* m_subsystem;
+ const std::string m_name;
+};
+
+enum Direction { kForward, kReverse };
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have
+ * multiple motors.
+ *
+ * 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.
+ *
+ *
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.
+ *
+ *
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.
+ */
+class SysIdRoutine : public frc::sysid::SysIdRoutineLog {
+ public:
+ /**
+ * Create a new SysId characterization routine.
+ *
+ * @param config Hardware-independent parameters for the SysId routine.
+ * @param mechanism Hardware interface for the SysId routine.
+ */
+ SysIdRoutine(Config config, Mechanism mechanism)
+ : SysIdRoutineLog(mechanism.m_subsystem->GetName()),
+ m_config(config),
+ m_mechanism(mechanism),
+ m_recordState(config.m_recordState ? config.m_recordState
+ : [this](frc::sysid::State state) {
+ this->RecordState(state);
+ }) {}
+
+ frc2::CommandPtr Quasistatic(Direction direction);
+ frc2::CommandPtr Dynamic(Direction direction);
+
+ private:
+ Config m_config;
+ Mechanism m_mechanism;
+ units::volt_t m_outputVolts{0};
+ std::function m_recordState;
+ frc::Timer timer;
+};
+} // namespace frc2::sysid
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java
new file mode 100644
index 00000000000..3d885b21bd9
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java
@@ -0,0 +1,144 @@
+// 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.Units.Volts;
+import static org.mockito.ArgumentMatchers.any;
+import static org.mockito.Mockito.atLeastOnce;
+import static org.mockito.Mockito.clearInvocations;
+import static org.mockito.Mockito.inOrder;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SysIdRoutineTest {
+ interface Mechanism extends Subsystem {
+ void recordState(SysIdRoutineLog.State state);
+
+ void drive(Measure voltage);
+
+ void log(SysIdRoutineLog log);
+ }
+
+ Mechanism m_mechanism;
+ SysIdRoutine m_sysidRoutine;
+ Command m_quasistaticForward;
+ Command m_quasistaticReverse;
+ Command m_dynamicForward;
+ Command m_dynamicReverse;
+
+ void runCommand(Command command) {
+ command.initialize();
+ command.execute();
+ command.execute();
+ SimHooks.stepTiming(1);
+ command.execute();
+ command.end(true);
+ }
+
+ @BeforeEach
+ void setup() {
+ HAL.initialize(500, 0);
+ SimHooks.pauseTiming();
+ m_mechanism = mock(Mechanism.class);
+ m_sysidRoutine =
+ new SysIdRoutine(
+ new SysIdRoutine.Config(null, null, null, m_mechanism::recordState),
+ new SysIdRoutine.Mechanism(m_mechanism::drive, m_mechanism::log, new Subsystem() {}));
+ m_quasistaticForward = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kForward);
+ m_quasistaticReverse = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kReverse);
+ m_dynamicForward = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kForward);
+ m_dynamicReverse = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kReverse);
+ }
+
+ @AfterEach
+ void cleanupAll() {
+ SimHooks.resumeTiming();
+ }
+
+ @Test
+ void recordStateBookendsMotorLogging() {
+ runCommand(m_quasistaticForward);
+
+ var orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+ orderCheck.verify(m_mechanism).drive(any());
+ orderCheck.verify(m_mechanism).log(any());
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+ orderCheck.verifyNoMoreInteractions();
+
+ clearInvocations(m_mechanism);
+ orderCheck = inOrder(m_mechanism);
+ runCommand(m_dynamicForward);
+
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kDynamicForward);
+ orderCheck.verify(m_mechanism).drive(any());
+ orderCheck.verify(m_mechanism).log(any());
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+ orderCheck.verifyNoMoreInteractions();
+ }
+
+ @Test
+ void testsDeclareCorrectState() {
+ runCommand(m_quasistaticForward);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+
+ runCommand(m_quasistaticReverse);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticReverse);
+
+ runCommand(m_dynamicForward);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicForward);
+
+ runCommand(m_dynamicReverse);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicReverse);
+ }
+
+ @Test
+ void testsOutputCorrectVoltage() {
+ runCommand(m_quasistaticForward);
+ var orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(1));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+
+ clearInvocations(m_mechanism);
+ runCommand(m_quasistaticReverse);
+ orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-1));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+
+ clearInvocations(m_mechanism);
+ runCommand(m_dynamicForward);
+ orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(7));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+
+ clearInvocations(m_mechanism);
+ runCommand(m_dynamicForward);
+ orderCheck = inOrder(m_mechanism);
+
+ runCommand(m_dynamicReverse);
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-7));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+ }
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
new file mode 100644
index 00000000000..5e4a5825ac3
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
@@ -0,0 +1,170 @@
+// 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
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+enum StateTest {
+ Invalid,
+ InRecordStateQf,
+ InRecordStateQr,
+ InRecordStateDf,
+ InRecordStateDr,
+ InDrive,
+ InLog,
+ DoneWithRecordState
+};
+
+class SysIdRoutineTest : public ::testing::Test {
+ protected:
+ std::vector currentStateList{};
+ std::vector sentVoltages{};
+ frc2::Subsystem m_subsystem{};
+ frc2::sysid::SysIdRoutine m_sysidRoutine{
+ frc2::sysid::Config{
+ std::nullopt, std::nullopt, std::nullopt,
+ [this](frc::sysid::State state) {
+ switch (state) {
+ case frc::sysid::State::kQuasistaticForward:
+ currentStateList.emplace_back(StateTest::InRecordStateQf);
+ break;
+ case frc::sysid::State::kQuasistaticReverse:
+ currentStateList.emplace_back(StateTest::InRecordStateQr);
+ break;
+ case frc::sysid::State::kDynamicForward:
+ currentStateList.emplace_back(StateTest::InRecordStateDf);
+ break;
+ case frc::sysid::State::kDynamicReverse:
+ currentStateList.emplace_back(StateTest::InRecordStateDr);
+ break;
+ case frc::sysid::State::kNone:
+ currentStateList.emplace_back(StateTest::DoneWithRecordState);
+ break;
+ }
+ }},
+ frc2::sysid::Mechanism{
+ [this](units::volt_t driveVoltage) {
+ sentVoltages.emplace_back(driveVoltage);
+ currentStateList.emplace_back(StateTest::InDrive);
+ },
+ [this](frc::sysid::SysIdRoutineLog* log) {
+ currentStateList.emplace_back(StateTest::InLog);
+ log->Motor("Mock Motor").position(0_m).velocity(0_mps).voltage(0_V);
+ },
+ &m_subsystem}};
+ frc2::CommandPtr m_quasistaticForward{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_quasistaticReverse{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+ frc2::CommandPtr m_dynamicForward{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_dynamicReverse{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+
+ void RunCommand(frc2::CommandPtr command) {
+ command.get()->Initialize();
+ command.get()->Execute();
+ frc::sim::StepTiming(1_s);
+ command.get()->Execute();
+ command.get()->End(true);
+ }
+
+ void SetUp() override {
+ frc::sim::PauseTiming();
+ frc2::CommandPtr m_quasistaticForward{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_quasistaticReverse{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+ frc2::CommandPtr m_dynamicForward{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_dynamicReverse{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+ }
+
+ void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+TEST_F(SysIdRoutineTest, RecordStateBookendsMotorLogging) {
+ RunCommand(std::move(m_quasistaticForward));
+ std::vector expectedOrder{
+ StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateQf,
+ StateTest::InDrive, StateTest::DoneWithRecordState};
+ EXPECT_TRUE(expectedOrder == currentStateList);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ expectedOrder = std::vector{
+ StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateDf,
+ StateTest::InDrive, StateTest::DoneWithRecordState};
+ RunCommand(std::move(m_dynamicForward));
+ EXPECT_TRUE(expectedOrder == currentStateList);
+ currentStateList.clear();
+ sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, DeclareCorrectState) {
+ RunCommand(std::move(m_quasistaticForward));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateQf) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_quasistaticReverse));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateQr) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicForward));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateDf) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicReverse));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateDr) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, OutputCorrectVoltage) {
+ RunCommand(std::move(m_quasistaticForward));
+ std::vector expectedVoltages{1_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_quasistaticReverse));
+ expectedVoltages = std::vector{-1_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicForward));
+ expectedVoltages = std::vector{7_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicReverse));
+ expectedVoltages = std::vector{-7_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+}
diff --git a/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp b/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
new file mode 100644
index 00000000000..0d16ddce9ec
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
@@ -0,0 +1,63 @@
+// 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 "frc/sysid/SysIdRoutineLog.h"
+
+#include "frc/DataLogManager.h"
+
+using namespace frc::sysid;
+
+SysIdRoutineLog::SysIdRoutineLog(const std::string& logName)
+ : m_logName(logName),
+ m_state(wpi::log::StringLogEntry{frc::DataLogManager::GetLog(),
+ "sysid-test-state" + logName}) {
+ m_state.Append(StateEnumToString(State::kNone));
+}
+
+SysIdRoutineLog::MotorLog::MotorLog(const std::string& motorName,
+ const std::string& logName,
+ LogEntries* logEntries)
+ : m_motorName(motorName), m_logName(logName), m_logEntries(logEntries) {
+ (*logEntries)[motorName] = MotorEntries();
+}
+
+SysIdRoutineLog::MotorLog& SysIdRoutineLog::MotorLog::value(
+ const std::string& name, double value, const std::string& unit) {
+ auto& motorEntries = (*m_logEntries)[m_motorName];
+
+ if (!motorEntries.contains(name)) {
+ wpi::log::DataLog& log = frc::DataLogManager::GetLog();
+
+ motorEntries[name] = wpi::log::DoubleLogEntry(
+ log, name + "-" + m_motorName + "-" + m_logName, unit);
+ }
+
+ motorEntries[name].Append(value);
+ return *this;
+}
+
+SysIdRoutineLog::MotorLog SysIdRoutineLog::Motor(const std::string& motorName) {
+ return MotorLog{motorName, m_logName, &m_logEntries};
+}
+
+void SysIdRoutineLog::RecordState(State state) {
+ m_state.Append(StateEnumToString(state));
+}
+
+std::string SysIdRoutineLog::StateEnumToString(State state) {
+ switch (state) {
+ case State::kQuasistaticForward:
+ return "quasistatic-forward";
+ case State::kQuasistaticReverse:
+ return "quasistatic-reverse";
+ case State::kDynamicForward:
+ return "dynamic-forward";
+ case State::kDynamicReverse:
+ return "dynamic-reverse";
+ case State::kNone:
+ return "none";
+ default:
+ return "none";
+ }
+}
diff --git a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h
new file mode 100644
index 00000000000..5bab3f95252
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h
@@ -0,0 +1,113 @@
+// 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.
+
+#pragma once
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace frc::sysid {
+
+/** Possible state of a SysId routine. */
+enum class State {
+ kQuasistaticForward,
+ kQuasistaticReverse,
+ kDynamicForward,
+ kDynamicReverse,
+ kNone
+};
+
+/**
+ * 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.
+ */
+class SysIdRoutineLog {
+ using MotorEntries = wpi::StringMap;
+ using LogEntries = wpi::StringMap;
+
+ public:
+ class MotorLog {
+ public:
+ MotorLog(const std::string& motorName, const std::string& logName,
+ LogEntries* logEntries);
+
+ MotorLog& value(const std::string& name, double value,
+ const std::string& unit);
+
+ MotorLog& voltage(units::volt_t voltage) {
+ return value("voltage", voltage.value(), voltage.name());
+ }
+
+ MotorLog& position(units::meter_t position) {
+ return value("position", position.value(), position.name());
+ }
+
+ MotorLog& position(units::turn_t position) {
+ return value("position", position.value(), position.name());
+ }
+
+ MotorLog& velocity(units::meters_per_second_t velocity) {
+ return value("velocity", velocity.value(), velocity.name());
+ }
+
+ MotorLog& velocity(units::turns_per_second_t velocity) {
+ return value("velocity", velocity.value(), velocity.name());
+ }
+
+ MotorLog& acceleration(units::meters_per_second_squared_t acceleration) {
+ return value("acceleration", acceleration.value(), acceleration.name());
+ }
+
+ MotorLog& acceleration(units::turns_per_second_squared_t acceleration) {
+ return value("acceleration", acceleration.value(), acceleration.name());
+ }
+
+ MotorLog& current(units::ampere_t current) {
+ return value("current", current.value(), current.name());
+ }
+
+ private:
+ std::string m_motorName;
+ std::string m_logName;
+ LogEntries* m_logEntries;
+ };
+ /**
+ * 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.
+ */
+ explicit SysIdRoutineLog(const std::string& logName);
+ /**
+ * 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.
+ */
+ void RecordState(State state);
+
+ MotorLog Motor(const std::string& motorName);
+
+ static std::string StateEnumToString(State state);
+
+ private:
+ LogEntries m_logEntries{};
+ std::string m_logName{};
+ wpi::log::StringLogEntry m_state{};
+};
+} // namespace frc::sysid
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp
new file mode 100644
index 00000000000..5ecb68a4d4b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp
@@ -0,0 +1,55 @@
+// 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 "Robot.h"
+
+#include
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+ frc2::CommandScheduler::GetInstance().Run();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_container.GetAutonomousCommand();
+
+ if (m_autonomousCommand) {
+ m_autonomousCommand->Schedule();
+ }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+ if (m_autonomousCommand) {
+ m_autonomousCommand->Cancel();
+ }
+}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {
+ frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+ return frc::StartRobot();
+}
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp
new file mode 100644
index 00000000000..21f6992ded1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp
@@ -0,0 +1,30 @@
+// 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 "SysIdRoutineBot.h"
+
+#include
+
+SysIdRoutineBot::SysIdRoutineBot() {
+ ConfigureBindings();
+}
+
+void SysIdRoutineBot::ConfigureBindings() {
+ m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(
+ [this] { return -m_driverController.GetLeftY(); },
+ [this] { return -m_driverController.GetRightX(); }));
+
+ m_driverController.A().WhileTrue(
+ m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+ m_driverController.B().WhileTrue(
+ m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+ m_driverController.X().WhileTrue(
+ m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+ m_driverController.Y().WhileTrue(
+ m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+}
+
+frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() {
+ return m_drive.Run([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp
new file mode 100644
index 00000000000..c7549761be1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp
@@ -0,0 +1,37 @@
+// 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 "subsystems/Drive.h"
+
+#include
+
+Drive::Drive() {
+ m_leftMotor.AddFollower(frc::PWMSparkMax{constants::drive::kLeftMotor2Port});
+ m_rightMotor.AddFollower(
+ frc::PWMSparkMax{constants::drive::kRightMotor2Port});
+
+ m_rightMotor.SetInverted(true);
+
+ m_leftEncoder.SetDistancePerPulse(
+ constants::drive::kEncoderDistancePerPulse.value());
+ m_rightEncoder.SetDistancePerPulse(
+ constants::drive::kEncoderDistancePerPulse.value());
+
+ m_drive.SetSafetyEnabled(false);
+}
+
+frc2::CommandPtr Drive::ArcadeDriveCommand(std::function fwd,
+ std::function rot) {
+ return frc2::cmd::Run([this, fwd, rot] { m_drive.ArcadeDrive(fwd(), rot()); },
+ {this})
+ .WithName("Arcade Drive");
+}
+
+frc2::CommandPtr Drive::SysIdQuasistatic(frc2::sysid::Direction direction) {
+ return m_sysIdRoutine.Quasistatic(direction);
+}
+
+frc2::CommandPtr Drive::SysIdDynamic(frc2::sysid::Direction direction) {
+ return m_sysIdRoutine.Dynamic(direction);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
new file mode 100644
index 00000000000..e01f533afee
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h
@@ -0,0 +1,81 @@
+// 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.
+
+#pragma once
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+namespace constants {
+namespace drive {
+inline constexpr int kLeftMotor1Port = 0;
+inline constexpr int kLeftMotor2Port = 1;
+inline constexpr int kRightMotor1Port = 2;
+inline constexpr int kRightMotor2Port = 3;
+
+inline constexpr std::array kLeftEncoderPorts = {0, 1};
+inline constexpr std::array kRightEncoderPorts = {2, 3};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kRightEncoderReversed = true;
+
+inline constexpr int kEncoderCpr = 1024;
+inline constexpr units::meter_t kWheelDiameter = 6_in;
+inline constexpr units::meter_t kEncoderDistancePerPulse =
+ (kWheelDiameter * std::numbers::pi) / static_cast(kEncoderCpr);
+} // namespace drive
+
+namespace shooter {
+
+using kv_unit =
+ units::compound_unit,
+ units::inverse>;
+using kv_unit_t = units::unit_t;
+
+inline constexpr std::array kEncoderPorts = {4, 5};
+inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr int kEncoderCpr = 1024;
+inline constexpr units::turn_t kEncoderDistancePerPulse =
+ units::turn_t{1.0} / static_cast(kEncoderCpr);
+
+inline constexpr int kShooterMotorPort = 4;
+inline constexpr int kFeederMotorPort = 5;
+
+inline constexpr units::turns_per_second_t kShooterFreeSpeed = 5300_tps;
+inline constexpr units::turns_per_second_t kShooterTargetSpeed = 4000_tps;
+inline constexpr units::turns_per_second_t kShooterTolerance = 50_tps;
+
+inline constexpr double kP = 1.0;
+
+inline constexpr units::volt_t kS = 0.05_V;
+inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+
+inline constexpr double kFeederSpeed = 0.5;
+} // namespace shooter
+
+namespace intake {
+inline constexpr int kMotorPort = 6;
+inline constexpr std::array kSolenoidPorts = {2, 3};
+} // namespace intake
+
+namespace storage {
+inline constexpr int kMotorPort = 7;
+inline constexpr int kBallSensorPort = 6;
+} // namespace storage
+
+namespace autonomous {
+inline constexpr units::second_t kTimeout = 3_s;
+inline constexpr units::meter_t kDriveDistance = 2_m;
+inline constexpr double kDriveSpeed = 0.5;
+} // namespace autonomous
+
+namespace oi {
+inline constexpr int kDriverControllerPort = 0;
+} // namespace oi
+} // namespace constants
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h
new file mode 100644
index 00000000000..b81adf12ec2
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h
@@ -0,0 +1,35 @@
+// 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.
+
+#pragma once
+
+#include
+
+#include
+#include
+
+#include "SysIdRoutineBot.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void DisabledExit() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void AutonomousExit() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TeleopExit() override;
+ void TestInit() override;
+ void TestPeriodic() override;
+ void TestExit() override;
+
+ private:
+ std::optional m_autonomousCommand;
+
+ SysIdRoutineBot m_container;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h
new file mode 100644
index 00000000000..6630abd3a6b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h
@@ -0,0 +1,24 @@
+// 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.
+
+#pragma once
+
+#include
+#include
+
+#include "Constants.h"
+#include "subsystems/Drive.h"
+
+class SysIdRoutineBot {
+ public:
+ SysIdRoutineBot();
+
+ frc2::CommandPtr GetAutonomousCommand();
+
+ private:
+ void ConfigureBindings();
+ frc2::CommandXboxController m_driverController{
+ constants::oi::kDriverControllerPort};
+ Drive m_drive{};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h
new file mode 100644
index 00000000000..61d34f131f7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h
@@ -0,0 +1,62 @@
+// 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.
+
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "Constants.h"
+
+class Drive : public frc2::SubsystemBase {
+ public:
+ Drive();
+
+ frc2::CommandPtr ArcadeDriveCommand(std::function fwd,
+ std::function rot);
+ frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+ frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
+ private:
+ frc::PWMSparkMax m_leftMotor{constants::drive::kLeftMotor1Port};
+ frc::PWMSparkMax m_rightMotor{constants::drive::kRightMotor1Port};
+ frc::DifferentialDrive m_drive{[this](auto val) { m_leftMotor.Set(val); },
+ [this](auto val) { m_rightMotor.Set(val); }};
+
+ frc::Encoder m_leftEncoder{constants::drive::kLeftEncoderPorts[0],
+ constants::drive::kLeftEncoderPorts[1],
+ constants::drive::kLeftEncoderReversed};
+
+ frc::Encoder m_rightEncoder{constants::drive::kRightEncoderPorts[0],
+ constants::drive::kRightEncoderPorts[1],
+ constants::drive::kRightEncoderReversed};
+
+ frc2::sysid::SysIdRoutine m_sysIdRoutine{
+ frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
+ std::nullopt},
+ frc2::sysid::Mechanism{
+ [this](units::volt_t driveVoltage) {
+ m_leftMotor.SetVoltage(driveVoltage);
+ m_rightMotor.SetVoltage(driveVoltage);
+ },
+ [this](frc::sysid::SysIdRoutineLog* log) {
+ log->Motor("drive-left")
+ .voltage(m_leftMotor.Get() *
+ frc::RobotController::GetBatteryVoltage())
+ .position(units::meter_t{m_leftEncoder.GetDistance()})
+ .velocity(units::meters_per_second_t{m_leftEncoder.GetRate()});
+ log->Motor("drive-right")
+ .voltage(m_rightMotor.Get() *
+ frc::RobotController::GetBatteryVoltage())
+ .position(units::meter_t{m_rightEncoder.GetDistance()})
+ .velocity(units::meters_per_second_t{m_rightEncoder.GetRate()});
+ },
+ this}};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
index 803290918cc..486e2ec62b1 100644
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -927,5 +927,17 @@
"foldername": "FlywheelBangBangController",
"gradlebase": "cpp",
"commandversion": 2
+ },
+ {
+ "name": "SysIdRoutine",
+ "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+ "tags": [
+ "SysId",
+ "Command-based",
+ "DataLog"
+ ],
+ "foldername": "SysId",
+ "gradlebase": "cpp",
+ "commandversion": 2
}
]
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java
new file mode 100644
index 00000000000..d21baf74fd6
--- /dev/null
+++ b/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> 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) {
+ 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 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 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) {
+ 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) {
+ 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>> 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>> 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) {
+ 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());
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 750e4dc7c22..b1ddb6c8406 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -983,5 +983,18 @@
"gradlebase": "java",
"commandversion": 2,
"mainclass": "Main"
+ },
+ {
+ "name": "SysIdRoutine",
+ "description": "A sample command-based robot demonstrating use of the SysIdRoutine command factory",
+ "tags": [
+ "SysId",
+ "Command-based",
+ "DataLog"
+ ],
+ "foldername": "sysid",
+ "gradlebase": "java",
+ "commandversion": 2,
+ "mainclass": "Main"
}
]
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
new file mode 100644
index 00000000000..285b98f4070
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java
@@ -0,0 +1,83 @@
+// 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.examples.sysid;
+
+import edu.wpi.first.math.util.Units;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class DriveConstants {
+ public static final int kLeftMotor1Port = 0;
+ public static final int kLeftMotor2Port = 1;
+ public static final int kRightMotor1Port = 2;
+ public static final int kRightMotor2Port = 3;
+
+ public static final int[] kLeftEncoderPorts = {0, 1};
+ public static final int[] kRightEncoderPorts = {2, 3};
+ public static final boolean kLeftEncoderReversed = false;
+ public static final boolean kRightEncoderReversed = true;
+
+ public static final int kEncoderCPR = 1024;
+ public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
+ public static final double kEncoderDistancePerPulse =
+ // Assumes the encoders are directly mounted on the wheel shafts
+ (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+ }
+
+ public static final class ShooterConstants {
+ public static final int[] kEncoderPorts = {4, 5};
+ public static final boolean kEncoderReversed = false;
+ public static final int kEncoderCPR = 1024;
+ public static final double kEncoderDistancePerPulse =
+ // Distance units will be rotations
+ 1.0 / (double) kEncoderCPR;
+
+ public static final int kShooterMotorPort = 4;
+ public static final int kFeederMotorPort = 5;
+
+ public static final double kShooterFreeRPS = 5300;
+ public static final double kShooterTargetRPS = 4000;
+ public static final double kShooterToleranceRPS = 50;
+
+ // These are not real PID gains, and will have to be tuned for your specific robot.
+ public static final double kP = 1;
+
+ // On a real robot the feedforward constants should be empirically determined; these are
+ // reasonable guesses.
+ public static final double kSVolts = 0.05;
+ public static final double kVVoltSecondsPerRotation =
+ // Should have value 12V at free speed...
+ 12.0 / kShooterFreeRPS;
+
+ public static final double kFeederSpeed = 0.5;
+ }
+
+ public static final class IntakeConstants {
+ public static final int kMotorPort = 6;
+ public static final int[] kSolenoidPorts = {2, 3};
+ }
+
+ public static final class StorageConstants {
+ public static final int kMotorPort = 7;
+ public static final int kBallSensorPort = 6;
+ }
+
+ public static final class AutoConstants {
+ public static final double kTimeoutSeconds = 3;
+ public static final double kDriveDistanceMeters = 2;
+ public static final double kDriveSpeed = 0.5;
+ }
+
+ public static final class OIConstants {
+ public static final int kDriverControllerPort = 0;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java
new file mode 100644
index 00000000000..1fdd4b7374c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java
@@ -0,0 +1,25 @@
+// 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.examples.sysid;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+ private Main() {}
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ *
If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java
new file mode 100644
index 00000000000..0b0f1428913
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java
@@ -0,0 +1,92 @@
+// 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.examples.sysid;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private Command m_autonomousCommand;
+
+ private final SysIdRoutineBot m_robot = new SysIdRoutineBot();
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Configure default commands and condition bindings on robot startup
+ m_robot.configureBindings();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ *
This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_robot.getAutonomousCommand();
+
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
new file mode 100644
index 00000000000..6665f3f7cb0
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java
@@ -0,0 +1,60 @@
+// 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.examples.sysid;
+
+import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants;
+
+import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class SysIdRoutineBot {
+ // The robot's subsystems
+ private final Drive m_drive = new Drive();
+
+ // The driver's controller
+ CommandXboxController m_driverController =
+ new CommandXboxController(OIConstants.kDriverControllerPort);
+
+ /**
+ * Use this method to define bindings between conditions and commands. These are useful for
+ * automating robot behaviors based on button and sensor input.
+ *
+ *
Should be called during {@link Robot#robotInit()}.
+ *
+ *
Event binding methods are available on the {@link Trigger} class.
+ */
+ public void configureBindings() {
+ // Control the drive with split-stick arcade controls
+ m_drive.setDefaultCommand(
+ m_drive.arcadeDriveCommand(
+ () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX()));
+
+ // Bind full set of SysId routine tests to buttons; a complete routine should run each of these
+ // once.
+ m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+ m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+ m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+ m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+ }
+
+ /**
+ * Use this to define the command that runs during autonomous.
+ *
+ *
Scheduled during {@link Robot#autonomousInit()}.
+ */
+ public Command getAutonomousCommand() {
+ // Do nothing
+ return m_drive.run(() -> {});
+ }
+}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java
new file mode 100644
index 00000000000..3c9610897c5
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java
@@ -0,0 +1,132 @@
+// 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.examples.sysid.subsystems;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.function.DoubleSupplier;
+
+public class Drive extends SubsystemBase {
+ // The motors on the left side of the drive.
+ private final PWMSparkMax m_leftMotor = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+
+ // The motors on the right side of the drive.
+ private final PWMSparkMax m_rightMotor = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+
+ // The robot's drive
+ private final DifferentialDrive m_drive =
+ new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
+
+ // The left-side drive encoder
+ private final Encoder m_leftEncoder =
+ new Encoder(
+ DriveConstants.kLeftEncoderPorts[0],
+ DriveConstants.kLeftEncoderPorts[1],
+ DriveConstants.kLeftEncoderReversed);
+
+ // The right-side drive encoder
+ private final Encoder m_rightEncoder =
+ new Encoder(
+ DriveConstants.kRightEncoderPorts[0],
+ DriveConstants.kRightEncoderPorts[1],
+ DriveConstants.kRightEncoderReversed);
+
+ // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+ private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0));
+ // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+ private final MutableMeasure m_distance = mutable(Meters.of(0));
+ // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+ private final MutableMeasure> m_velocity = mutable(MetersPerSecond.of(0));
+
+ // Create a new SysId routine for characterizing the drive.
+ private final SysIdRoutine m_sysIdRoutine =
+ new SysIdRoutine(
+ // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+ new SysIdRoutine.Config(),
+ new SysIdRoutine.Mechanism(
+ // Tell SysId how to plumb the driving voltage to the motors.
+ (Measure volts) -> {
+ m_leftMotor.setVoltage(volts.in(Volts));
+ m_rightMotor.setVoltage(volts.in(Volts));
+ },
+ // Tell SysId how to record a frame of data for each motor on the mechanism being
+ // characterized.
+ log -> {
+ // Record a frame for the left motors. Since these share an encoder, we consider
+ // the entire group to be one motor.
+ log.motor("drive-left")
+ .voltage(
+ m_appliedVoltage.mut_replace(
+ m_leftMotor.get() * RobotController.getBatteryVoltage(), Volts))
+ .linearPosition(m_distance.mut_replace(m_leftEncoder.getDistance(), Meters))
+ .linearVelocity(
+ m_velocity.mut_replace(m_leftEncoder.getRate(), MetersPerSecond));
+ // Record a frame for the right motors. Since these share an encoder, we consider
+ // the entire group to be one motor.
+ log.motor("drive-right")
+ .voltage(
+ m_appliedVoltage.mut_replace(
+ m_rightMotor.get() * RobotController.getBatteryVoltage(), Volts))
+ .linearPosition(m_distance.mut_replace(m_rightEncoder.getDistance(), Meters))
+ .linearVelocity(
+ m_velocity.mut_replace(m_rightEncoder.getRate(), MetersPerSecond));
+ },
+ // Tell SysId to make generated commands require this subsystem, suffix test state in
+ // WPILog with this subsystem's name ("drive")
+ this));
+
+ /** Creates a new Drive subsystem. */
+ public Drive() {
+ // Add the second motors on each side of the drivetrain
+ m_leftMotor.addFollower(new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+ m_rightMotor.addFollower(new PWMSparkMax(DriveConstants.kRightMotor2Port));
+
+ // We need to invert one side of the drivetrain so that positive voltages
+ // result in both sides moving forward. Depending on how your robot's
+ // gearbox is constructed, you might have to invert the left side instead.
+ m_rightMotor.setInverted(true);
+
+ // Sets the distance per pulse for the encoders
+ m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+ }
+
+ /**
+ * Returns a command that drives the robot with arcade controls.
+ *
+ * @param fwd the commanded forward movement
+ * @param rot the commanded rotation
+ */
+ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
+ // A split-stick arcade command, with forward/backward controlled by the left
+ // hand, and turning controlled by the right.
+ return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
+ .withName("arcadeDrive");
+ }
+
+ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+ return m_sysIdRoutine.quasistatic(direction);
+ }
+
+ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+ return m_sysIdRoutine.dynamic(direction);
+ }
+}