From 707cb061057f3e9560fa090c1c9b9410cf73f782 Mon Sep 17 00:00:00 2001 From: Eli Barnett Date: Fri, 5 Jan 2024 14:50:23 -0500 Subject: [PATCH] [wpilib] Add SysIdRoutine logging utility and command factory (#6033) Co-authored-by: Drew Williams Co-authored-by: Peter Johnson Co-authored-by: Tyler Veness --- .../wpilibj2/command/sysid/SysIdRoutine.java | 264 ++++++++++++++++++ .../cpp/frc2/command/sysid/SysIdRoutine.cpp | 64 +++++ .../include/frc2/command/sysid/SysIdRoutine.h | 162 +++++++++++ .../command/sysid/SysIdRoutineTest.java | 144 ++++++++++ .../frc2/command/sysid/SysIdRoutineTest.cpp | 170 +++++++++++ .../main/native/cpp/sysid/SysIdRoutineLog.cpp | 63 +++++ .../include/frc/sysid/SysIdRoutineLog.h | 113 ++++++++ .../src/main/cpp/examples/SysId/cpp/Robot.cpp | 55 ++++ .../examples/SysId/cpp/SysIdRoutineBot.cpp | 30 ++ .../examples/SysId/cpp/subsystems/Drive.cpp | 37 +++ .../cpp/examples/SysId/include/Constants.h | 81 ++++++ .../main/cpp/examples/SysId/include/Robot.h | 35 +++ .../examples/SysId/include/SysIdRoutineBot.h | 24 ++ .../examples/SysId/include/subsystems/Drive.h | 62 ++++ .../src/main/cpp/examples/examples.json | 12 + .../first/wpilibj/sysid/SysIdRoutineLog.java | 215 ++++++++++++++ .../wpi/first/wpilibj/examples/examples.json | 13 + .../wpilibj/examples/sysid/Constants.java | 83 ++++++ .../first/wpilibj/examples/sysid/Main.java | 25 ++ .../first/wpilibj/examples/sysid/Robot.java | 92 ++++++ .../examples/sysid/SysIdRoutineBot.java | 60 ++++ .../examples/sysid/subsystems/Drive.java | 132 +++++++++ 22 files changed, 1936 insertions(+) create mode 100644 wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java create mode 100644 wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp create mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h create mode 100644 wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java create mode 100644 wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp create mode 100644 wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp create mode 100644 wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Drive.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Drive.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java 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

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); + } +}