From 215b5c6ad8640dc37f3d4788869144a6f9b824c7 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 4 Aug 2023 13:42:25 -0700 Subject: [PATCH] Add Requirements struct --- .../main/native/cpp/frc2/command/Command.cpp | 25 +-- .../native/cpp/frc2/command/CommandPtr.cpp | 22 +- .../main/native/cpp/frc2/command/Commands.cpp | 36 +--- .../cpp/frc2/command/FunctionalCommand.cpp | 13 +- .../cpp/frc2/command/InstantCommand.cpp | 8 +- .../frc2/command/MecanumControllerCommand.cpp | 124 +----------- .../cpp/frc2/command/NotifierCommand.cpp | 9 +- .../native/cpp/frc2/command/PIDCommand.cpp | 24 +-- .../cpp/frc2/command/RamseteCommand.cpp | 42 +--- .../native/cpp/frc2/command/RunCommand.cpp | 8 +- .../cpp/frc2/command/StartEndCommand.cpp | 10 +- .../native/include/frc2/command/Command.h | 49 +---- .../native/include/frc2/command/CommandPtr.h | 26 +-- .../native/include/frc2/command/Commands.h | 53 +---- .../include/frc2/command/FunctionalCommand.h | 18 +- .../include/frc2/command/InstantCommand.h | 12 +- .../frc2/command/MecanumControllerCommand.h | 190 +----------------- .../include/frc2/command/NotifierCommand.h | 12 +- .../native/include/frc2/command/PIDCommand.h | 35 +--- .../include/frc2/command/ProfiledPIDCommand.h | 89 +------- .../include/frc2/command/RamseteCommand.h | 65 +----- .../include/frc2/command/Requirements.h | 46 +++++ .../native/include/frc2/command/RunCommand.h | 12 +- .../include/frc2/command/StartEndCommand.h | 13 +- .../frc2/command/SwerveControllerCommand.h | 146 +------------- .../frc2/command/SwerveControllerCommand.inc | 74 +------ .../frc2/command/TrapezoidProfileCommand.h | 48 +---- .../cpp/frc2/command/AddRequirementsTest.cpp | 144 +++++++++++++ .../GearsBot/cpp/commands/CloseClaw.cpp | 2 +- .../GearsBot/cpp/commands/OpenClaw.cpp | 2 +- .../cpp/commands/SetElevatorSetpoint.cpp | 2 +- .../cpp/commands/SetWristSetpoint.cpp | 2 +- .../GearsBot/cpp/commands/TankDrive.cpp | 2 +- .../cpp/commands/TurnToAngle.cpp | 2 +- .../cpp/commands/TurnToAngleProfiled.cpp | 2 +- .../cpp/commands/DefaultDrive.cpp | 2 +- .../cpp/commands/DriveDistance.cpp | 2 +- .../cpp/commands/ReleaseHatch.cpp | 2 +- .../cpp/commands/TeleopArcadeDrive.cpp | 2 +- .../include/commands/DriveDistance.h | 2 +- .../include/commands/DriveTime.h | 2 +- .../include/commands/TurnDegrees.h | 2 +- .../RomiReference/include/commands/TurnTime.h | 2 +- 43 files changed, 272 insertions(+), 1111 deletions(-) create mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/Requirements.h create mode 100644 wpilibNewCommands/src/test/native/cpp/frc2/command/AddRequirementsTest.cpp diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index 97a25d2eb2c..7ea3a1449c6 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -43,11 +43,7 @@ wpi::SmallSet Command::GetRequirements() const { return m_requirements; } -void Command::AddRequirements(std::initializer_list requirements) { - m_requirements.insert(requirements.begin(), requirements.end()); -} - -void Command::AddRequirements(std::span requirements) { +void Command::AddRequirements(Requirements requirements) { m_requirements.insert(requirements.begin(), requirements.end()); } @@ -96,27 +92,14 @@ CommandPtr Command::WithInterruptBehavior( return std::move(*this).ToPtr().WithInterruptBehavior(interruptBehavior); } -CommandPtr Command::BeforeStarting( - std::function toRun, - std::initializer_list requirements) && { - return std::move(*this).BeforeStarting( - std::move(toRun), {requirements.begin(), requirements.end()}); -} - -CommandPtr Command::BeforeStarting( - std::function toRun, std::span requirements) && { +CommandPtr Command::BeforeStarting(std::function toRun, + Requirements requirements) && { return std::move(*this).ToPtr().BeforeStarting(std::move(toRun), requirements); } CommandPtr Command::AndThen(std::function toRun, - std::initializer_list requirements) && { - return std::move(*this).AndThen(std::move(toRun), - {requirements.begin(), requirements.end()}); -} - -CommandPtr Command::AndThen(std::function toRun, - std::span requirements) && { + Requirements requirements) && { return std::move(*this).ToPtr().AndThen(std::move(toRun), requirements); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp index 6b2b86e3c20..050f95218de 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp @@ -86,15 +86,7 @@ CommandPtr CommandPtr::WithInterruptBehavior( } CommandPtr CommandPtr::AndThen(std::function toRun, - std::span requirements) && { - AssertValid(); - return std::move(*this).AndThen(CommandPtr( - std::make_unique(std::move(toRun), requirements))); -} - -CommandPtr CommandPtr::AndThen( - std::function toRun, - std::initializer_list requirements) && { + Requirements requirements) && { AssertValid(); return std::move(*this).AndThen(CommandPtr( std::make_unique(std::move(toRun), requirements))); @@ -109,16 +101,8 @@ CommandPtr CommandPtr::AndThen(CommandPtr&& next) && { return std::move(*this); } -CommandPtr CommandPtr::BeforeStarting( - std::function toRun, std::span requirements) && { - AssertValid(); - return std::move(*this).BeforeStarting(CommandPtr( - std::make_unique(std::move(toRun), requirements))); -} - -CommandPtr CommandPtr::BeforeStarting( - std::function toRun, - std::initializer_list requirements) && { +CommandPtr CommandPtr::BeforeStarting(std::function toRun, + Requirements requirements) && { AssertValid(); return std::move(*this).BeforeStarting(CommandPtr( std::make_unique(std::move(toRun), requirements))); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp index f3c47765322..5c847b1d196 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp @@ -29,36 +29,16 @@ CommandPtr cmd::Idle() { } CommandPtr cmd::RunOnce(std::function action, - std::initializer_list requirements) { + Requirements requirements) { return InstantCommand(std::move(action), requirements).ToPtr(); } -CommandPtr cmd::RunOnce(std::function action, - std::span requirements) { - return InstantCommand(std::move(action), requirements).ToPtr(); -} - -CommandPtr cmd::Run(std::function action, - std::initializer_list requirements) { +CommandPtr cmd::Run(std::function action, Requirements requirements) { return RunCommand(std::move(action), requirements).ToPtr(); } -CommandPtr cmd::Run(std::function action, - std::span requirements) { - return RunCommand(std::move(action), requirements).ToPtr(); -} - -CommandPtr cmd::StartEnd(std::function start, std::function end, - std::initializer_list requirements) { - return FunctionalCommand( - std::move(start), [] {}, - [end = std::move(end)](bool interrupted) { end(); }, - [] { return false; }, requirements) - .ToPtr(); -} - CommandPtr cmd::StartEnd(std::function start, std::function end, - std::span requirements) { + Requirements requirements) { return FunctionalCommand( std::move(start), [] {}, [end = std::move(end)](bool interrupted) { end(); }, @@ -67,15 +47,7 @@ CommandPtr cmd::StartEnd(std::function start, std::function end, } CommandPtr cmd::RunEnd(std::function run, std::function end, - std::initializer_list requirements) { - return FunctionalCommand([] {}, std::move(run), - [end = std::move(end)](bool interrupted) { end(); }, - [] { return false; }, requirements) - .ToPtr(); -} - -CommandPtr cmd::RunEnd(std::function run, std::function end, - std::span requirements) { + Requirements requirements) { return FunctionalCommand([] {}, std::move(run), [end = std::move(end)](bool interrupted) { end(); }, [] { return false; }, requirements) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp index 02e83385c5a..e4ff6ea4174 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp @@ -6,22 +6,11 @@ using namespace frc2; -FunctionalCommand::FunctionalCommand( - std::function onInit, std::function onExecute, - std::function onEnd, std::function isFinished, - std::initializer_list requirements) - : m_onInit{std::move(onInit)}, - m_onExecute{std::move(onExecute)}, - m_onEnd{std::move(onEnd)}, - m_isFinished{std::move(isFinished)} { - AddRequirements(requirements); -} - FunctionalCommand::FunctionalCommand(std::function onInit, std::function onExecute, std::function onEnd, std::function isFinished, - std::span requirements) + Requirements requirements) : m_onInit{std::move(onInit)}, m_onExecute{std::move(onExecute)}, m_onEnd{std::move(onEnd)}, diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp index 7b7323993fb..b78d30e0e88 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp @@ -7,13 +7,7 @@ using namespace frc2; InstantCommand::InstantCommand(std::function toRun, - std::initializer_list requirements) - : CommandHelper( - std::move(toRun), [] {}, [](bool interrupted) {}, [] { return true; }, - requirements) {} - -InstantCommand::InstantCommand(std::function toRun, - std::span requirements) + Requirements requirements) : CommandHelper( std::move(toRun), [] {}, [](bool interrupted) {}, [] { return true; }, requirements) {} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index abd8ca5d039..902c26b30eb 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -24,7 +24,7 @@ MecanumControllerCommand::MecanumControllerCommand( std::function output, - std::initializer_list requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_feedforward(feedforward), @@ -61,7 +61,7 @@ MecanumControllerCommand::MecanumControllerCommand( std::function output, - std::initializer_list requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_feedforward(feedforward), @@ -82,122 +82,6 @@ MecanumControllerCommand::MecanumControllerCommand( AddRequirements(requirements); } -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - output, - std::span requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_maxWheelVelocity(maxWheelVelocity), - m_frontLeftController( - std::make_unique(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - output, - std::span requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(feedforward), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_maxWheelVelocity(maxWheelVelocity), - m_frontLeftController( - std::make_unique(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::initializer_list requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_maxWheelVelocity(maxWheelVelocity), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::initializer_list requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_maxWheelVelocity(maxWheelVelocity), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - MecanumControllerCommand::MecanumControllerCommand( frc::Trajectory trajectory, std::function pose, frc::MecanumDriveKinematics kinematics, frc::PIDController xController, @@ -208,7 +92,7 @@ MecanumControllerCommand::MecanumControllerCommand( std::function output, - std::span requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_kinematics(kinematics), @@ -229,7 +113,7 @@ MecanumControllerCommand::MecanumControllerCommand( std::function output, - std::span requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_kinematics(kinematics), diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp index 2081d073dc9..5dc5add96f5 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp @@ -8,14 +8,7 @@ using namespace frc2; NotifierCommand::NotifierCommand(std::function toRun, units::second_t period, - std::initializer_list requirements) - : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { - AddRequirements(requirements); -} - -NotifierCommand::NotifierCommand(std::function toRun, - units::second_t period, - std::span requirements) + Requirements requirements) : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} { AddRequirements(requirements); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp index dd9e3238285..361ec841cc6 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp @@ -12,7 +12,7 @@ PIDCommand::PIDCommand(frc::PIDController controller, std::function measurementSource, std::function setpointSource, std::function useOutput, - std::initializer_list requirements) + Requirements requirements) : m_controller{std::move(controller)}, m_measurement{std::move(measurementSource)}, m_setpoint{std::move(setpointSource)}, @@ -20,30 +20,10 @@ PIDCommand::PIDCommand(frc::PIDController controller, AddRequirements(requirements); } -PIDCommand::PIDCommand(frc::PIDController controller, - std::function measurementSource, - std::function setpointSource, - std::function useOutput, - std::span requirements) - : m_controller{std::move(controller)}, - m_measurement{std::move(measurementSource)}, - m_setpoint{std::move(setpointSource)}, - m_useOutput{std::move(useOutput)} { - AddRequirements(requirements); -} - -PIDCommand::PIDCommand(frc::PIDController controller, - std::function measurementSource, - double setpoint, std::function useOutput, - std::initializer_list requirements) - : PIDCommand( - controller, measurementSource, [setpoint] { return setpoint; }, - useOutput, requirements) {} - PIDCommand::PIDCommand(frc::PIDController controller, std::function measurementSource, double setpoint, std::function useOutput, - std::span requirements) + Requirements requirements) : PIDCommand( controller, measurementSource, [setpoint] { return setpoint; }, useOutput, requirements) {} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index 7f8adeb593f..91459858dd6 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -18,7 +18,7 @@ RamseteCommand::RamseteCommand( std::function wheelSpeeds, frc::PIDController leftController, frc::PIDController rightController, std::function output, - std::initializer_list requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_controller(controller), @@ -32,51 +32,13 @@ RamseteCommand::RamseteCommand( AddRequirements(requirements); } -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc::PIDController leftController, frc::PIDController rightController, - std::function output, - std::span requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_feedforward(feedforward), - m_kinematics(std::move(kinematics)), - m_speeds(std::move(wheelSpeeds)), - m_leftController(std::make_unique(leftController)), - m_rightController(std::make_unique(rightController)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -RamseteCommand::RamseteCommand( - frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - std::initializer_list requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_controller(controller), - m_kinematics(std::move(kinematics)), - m_outputVel(std::move(output)), - m_usePID(false) { - AddRequirements(requirements); -} - RamseteCommand::RamseteCommand( frc::Trajectory trajectory, std::function pose, frc::RamseteController controller, frc::DifferentialDriveKinematics kinematics, std::function output, - std::span requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_controller(controller), diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp index 342362febee..c3378eaf1f0 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp @@ -6,12 +6,6 @@ using namespace frc2; -RunCommand::RunCommand(std::function toRun, - std::initializer_list requirements) - : CommandHelper([] {}, std::move(toRun), [](bool interrupted) {}, - [] { return false; }, requirements) {} - -RunCommand::RunCommand(std::function toRun, - std::span requirements) +RunCommand::RunCommand(std::function toRun, Requirements requirements) : CommandHelper([] {}, std::move(toRun), [](bool interrupted) {}, [] { return false; }, requirements) {} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp index 3d91ac5dd6d..2d257a3bc7e 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp @@ -8,15 +8,7 @@ using namespace frc2; StartEndCommand::StartEndCommand(std::function onInit, std::function onEnd, - std::initializer_list requirements) - : CommandHelper( - std::move(onInit), [] {}, - [onEnd = std::move(onEnd)](bool interrupted) { onEnd(); }, - [] { return false; }, requirements) {} - -StartEndCommand::StartEndCommand(std::function onInit, - std::function onEnd, - std::span requirements) + Requirements requirements) : CommandHelper( std::move(onInit), [] {}, [onEnd = std::move(onEnd)](bool interrupted) { onEnd(); }, diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 2ad19fd39a0..0d13ef33269 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -15,6 +15,7 @@ #include #include +#include "frc2/command/Requirements.h" #include "frc2/command/Subsystem.h" namespace frc2 { @@ -104,23 +105,15 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * is scheduled, so this method should normally be called from the command's * constructor. * - * @param requirements the Subsystem requirements to add - */ - void AddRequirements(std::initializer_list requirements); - - /** - * Adds the specified Subsystem requirements to the command. + * While this overload can be used with {@code AddRequirements({&subsystem1, + * &subsystem2})}, {@code AddRequirements({&subsystem})} selects the {@code + * AddRequirements(Subsystem*)} overload, which will function identically but + * may cause warnings about redundant braces. * - * The scheduler will prevent two commands that require the same subsystem - * from being scheduled simultaneously. - * - * Note that the scheduler determines the requirements of a command when it - * is scheduled, so this method should normally be called from the command's - * constructor. - * - * @param requirements the Subsystem requirements to add + * @param requirements the Subsystem requirements to add, which can be + * implicitly constructed from an initializer list or a span */ - void AddRequirements(std::span requirements); + void AddRequirements(Requirements requirements); /** * Adds the specified Subsystem requirements to the command. @@ -238,29 +231,7 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { */ [[nodiscard]] CommandPtr BeforeStarting(std::function toRun, - std::initializer_list requirements) &&; - - /** - * Decorates this command with a runnable to run before this command starts. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] - CommandPtr BeforeStarting(std::function toRun, - std::span requirements = {}) &&; - - /** - * Decorates this command with a runnable to run after the command finishes. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] - CommandPtr AndThen(std::function toRun, - std::initializer_list requirements) &&; + Requirements requirements = {}) &&; /** * Decorates this command with a runnable to run after the command finishes. @@ -271,7 +242,7 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { */ [[nodiscard]] CommandPtr AndThen(std::function toRun, - std::span requirements = {}) &&; + Requirements requirements = {}) &&; /** * Decorates this command to run repeatedly, restarting it when it ends, until diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h index bb344a35053..fa29f9a7e40 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h @@ -88,18 +88,7 @@ class CommandPtr final { */ [[nodiscard]] CommandPtr AndThen(std::function toRun, - std::span requirements = {}) &&; - - /** - * Decorates this command with a runnable to run after the command finishes. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] - CommandPtr AndThen(std::function toRun, - std::initializer_list requirements) &&; + Requirements requirements = {}) &&; /** * Decorates this command with a set of commands to run after it in sequence. @@ -121,18 +110,7 @@ class CommandPtr final { */ [[nodiscard]] CommandPtr BeforeStarting(std::function toRun, - std::initializer_list requirements) &&; - - /** - * Decorates this command with a runnable to run before this command starts. - * - * @param toRun the Runnable to run - * @param requirements the required subsystems - * @return the decorated command - */ - [[nodiscard]] - CommandPtr BeforeStarting(std::function toRun, - std::span requirements = {}) &&; + Requirements requirements = {}) &&; /** * Decorates this command with another command to run before this command diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h index f31bab98e05..94ae8dce263 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h @@ -49,27 +49,7 @@ CommandPtr Idle(); */ [[nodiscard]] CommandPtr RunOnce(std::function action, - std::initializer_list requirements); - -/** - * Constructs a command that runs an action once and finishes. - * - * @param action the action to run - * @param requirements subsystems the action requires - */ -[[nodiscard]] -CommandPtr RunOnce(std::function action, - std::span requirements = {}); - -/** - * Constructs a command that runs an action every iteration until interrupted. - * - * @param action the action to run - * @param requirements subsystems the action requires - */ -[[nodiscard]] -CommandPtr Run(std::function action, - std::initializer_list requirements); + Requirements requirements = {}); /** * Constructs a command that runs an action every iteration until interrupted. @@ -78,8 +58,7 @@ CommandPtr Run(std::function action, * @param requirements subsystems the action requires */ [[nodiscard]] -CommandPtr Run(std::function action, - std::span requirements = {}); +CommandPtr Run(std::function action, Requirements requirements = {}); /** * Constructs a command that runs an action once and another action when the @@ -91,31 +70,7 @@ CommandPtr Run(std::function action, */ [[nodiscard]] CommandPtr StartEnd(std::function start, std::function end, - std::initializer_list requirements); - -/** - * Constructs a command that runs an action once and another action when the - * command is interrupted. - * - * @param start the action to run on start - * @param end the action to run on interrupt - * @param requirements subsystems the action requires - */ -[[nodiscard]] -CommandPtr StartEnd(std::function start, std::function end, - std::span requirements = {}); - -/** - * Constructs a command that runs an action every iteration until interrupted, - * and then runs a second action. - * - * @param run the action to run every iteration - * @param end the action to run on interrupt - * @param requirements subsystems the action requires - */ -[[nodiscard]] -CommandPtr RunEnd(std::function run, std::function end, - std::initializer_list requirements); + Requirements requirements = {}); /** * Constructs a command that runs an action every iteration until interrupted, @@ -127,7 +82,7 @@ CommandPtr RunEnd(std::function run, std::function end, */ [[nodiscard]] CommandPtr RunEnd(std::function run, std::function end, - std::span requirements = {}); + Requirements requirements = {}); /** * Constructs a command that prints a message and finishes. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h index 223f4c2f6a7..5fa57f17137 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h @@ -37,23 +37,7 @@ class FunctionalCommand : public CommandHelper { std::function onExecute, std::function onEnd, std::function isFinished, - std::initializer_list requirements); - - /** - * Creates a new FunctionalCommand. - * - * @param onInit the function to run on command initialization - * @param onExecute the function to run on command execution - * @param onEnd the function to run on command end - * @param isFinished the function that determines whether the command has - * finished - * @param requirements the subsystems required by this command - */ - FunctionalCommand(std::function onInit, - std::function onExecute, - std::function onEnd, - std::function isFinished, - std::span requirements = {}); + Requirements requirements = {}); FunctionalCommand(FunctionalCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h index cae0b3e0e77..1e552cc4473 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h @@ -21,16 +21,6 @@ namespace frc2 { */ class InstantCommand : public CommandHelper { public: - /** - * Creates a new InstantCommand that runs the given Runnable with the given - * requirements. - * - * @param toRun the Runnable to run - * @param requirements the subsystems required by this command - */ - InstantCommand(std::function toRun, - std::initializer_list requirements); - /** * Creates a new InstantCommand that runs the given Runnable with the given * requirements. @@ -39,7 +29,7 @@ class InstantCommand : public CommandHelper { * @param requirements the subsystems required by this command */ explicit InstantCommand(std::function toRun, - std::span requirements = {}); + Requirements requirements = {}); InstantCommand(InstantCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h index 8946d3ff198..9a88186dcda 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h @@ -102,7 +102,7 @@ class MecanumControllerCommand std::function output, - std::initializer_list requirements); + Requirements requirements = {}); /** * Constructs a new MecanumControllerCommand that when executed will follow @@ -155,111 +155,7 @@ class MecanumControllerCommand std::function output, - std::initializer_list requirements); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. PID control and feedforward are handled - * internally. Outputs are scaled from -12 to 12 as a voltage output to the - * motor. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param feedforward The feedforward to use for the drivetrain. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the robot should be facing. - * This is sampled at each time step. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param frontLeftController The front left wheel velocity PID. - * @param rearLeftController The rear left wheel velocity PID. - * @param frontRightController The front right wheel velocity PID. - * @param rearRightController The rear right wheel velocity PID. - * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing - * the current wheel speeds. - * @param output The output of the velocity PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - output, - std::span requirements = {}); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. PID control and feedforward are handled - * internally. Outputs are scaled from -12 to 12 as a voltage output to the - * motor. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param feedforward The feedforward to use for the drivetrain. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param frontLeftController The front left wheel velocity PID. - * @param rearLeftController The rear left wheel velocity PID. - * @param frontRightController The front right wheel velocity PID. - * @param rearRightController The rear right wheel velocity PID. - * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing - * the current wheel speeds. - * @param output The output of the velocity PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - output, - std::span requirements = {}); + Requirements requirements = {}); /** * Constructs a new MecanumControllerCommand that when executed will follow @@ -297,7 +193,7 @@ class MecanumControllerCommand units::meters_per_second_t, units::meters_per_second_t)> output, - std::initializer_list requirements); + Requirements requirements); /** * Constructs a new MecanumControllerCommand that when executed will follow @@ -337,85 +233,7 @@ class MecanumControllerCommand units::meters_per_second_t, units::meters_per_second_t)> output, - std::initializer_list requirements); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. The user should implement a velocity PID on the - * desired output wheel velocities. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path - this is left to the user, since it is not - * appropriate for paths with nonstationary end-states. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one - * of the odometry classes to provide this. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the robot should be facing. - * This is sampled at every time step. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param output The output of the position PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::span requirements = {}); - - /** - * Constructs a new MecanumControllerCommand that when executed will follow - * the provided trajectory. The user should implement a velocity PID on the - * desired output wheel velocities. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path - this is left to the user, since it is not - * appropriate for paths with nonstationary end-states. - * - *

Note2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one - * of the odometry classes to provide this. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. - * @param output The output of the position PIDs. - * @param requirements The subsystems to require. - */ - MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - std::span requirements = {}); + Requirements requirements = {}); void Initialize() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h index 7db5e3fa36a..9bb063f56a3 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h @@ -37,17 +37,7 @@ class NotifierCommand : public CommandHelper { * @param requirements the subsystems required by this command */ NotifierCommand(std::function toRun, units::second_t period, - std::initializer_list requirements); - - /** - * Creates a new NotifierCommand. - * - * @param toRun the runnable for the notifier to run - * @param period the period at which the notifier should run - * @param requirements the subsystems required by this command - */ - NotifierCommand(std::function toRun, units::second_t period, - std::span requirements = {}); + Requirements requirements = {}); NotifierCommand(NotifierCommand&& other); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h index ebc51687252..4d2648b298b 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h @@ -40,38 +40,7 @@ class PIDCommand : public CommandHelper { std::function measurementSource, std::function setpointSource, std::function useOutput, - std::initializer_list requirements); - - /** - * Creates a new PIDCommand, which controls the given output with a - * PIDController. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param setpointSource the controller's reference (aka setpoint) - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - PIDCommand(frc::PIDController controller, - std::function measurementSource, - std::function setpointSource, - std::function useOutput, - std::span requirements = {}); - - /** - * Creates a new PIDCommand, which controls the given output with a - * PIDController with a constant setpoint. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param setpoint the controller's setpoint (aka setpoint) - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - PIDCommand(frc::PIDController controller, - std::function measurementSource, double setpoint, - std::function useOutput, - std::initializer_list requirements); + Requirements requirements = {}); /** * Creates a new PIDCommand, which controls the given output with a @@ -86,7 +55,7 @@ class PIDCommand : public CommandHelper { PIDCommand(frc::PIDController controller, std::function measurementSource, double setpoint, std::function useOutput, - std::span requirements = {}); + Requirements requirements = {}); PIDCommand(PIDCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h index 596ac1bda6a..01aa90a8ee6 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h @@ -50,29 +50,7 @@ class ProfiledPIDCommand std::function measurementSource, std::function goalSource, std::function useOutput, - std::initializer_list requirements) - : m_controller{controller}, - m_measurement{std::move(measurementSource)}, - m_goal{std::move(goalSource)}, - m_useOutput{std::move(useOutput)} { - this->AddRequirements(requirements); - } - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goalSource the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - std::function goalSource, - std::function useOutput, - std::span requirements = {}) + Requirements requirements = {}) : m_controller{controller}, m_measurement{std::move(measurementSource)}, m_goal{std::move(goalSource)}, @@ -94,7 +72,7 @@ class ProfiledPIDCommand std::function measurementSource, std::function goalSource, std::function useOutput, - std::initializer_list requirements) + Requirements requirements = {}) : ProfiledPIDCommand( controller, measurementSource, [goalSource = std::move(goalSource)]() { @@ -102,46 +80,6 @@ class ProfiledPIDCommand }, useOutput, requirements) {} - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goalSource the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - std::function goalSource, - std::function useOutput, - std::span requirements = {}) - : ProfiledPIDCommand( - controller, measurementSource, - [goalSource = std::move(goalSource)]() { - return State{goalSource(), Velocity_t{0}}; - }, - useOutput, requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController with a constant goal. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goal the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, State goal, - std::function useOutput, - std::initializer_list requirements) - : ProfiledPIDCommand( - controller, measurementSource, [goal] { return goal; }, useOutput, - requirements) {} - /** * Creates a new PIDCommand, which controls the given output with a * ProfiledPIDController with a constant goal. @@ -155,26 +93,7 @@ class ProfiledPIDCommand ProfiledPIDCommand(frc::ProfiledPIDController controller, std::function measurementSource, State goal, std::function useOutput, - std::span requirements = {}) - : ProfiledPIDCommand( - controller, measurementSource, [goal] { return goal; }, useOutput, - requirements) {} - - /** - * Creates a new PIDCommand, which controls the given output with a - * ProfiledPIDController with a constant goal. - * - * @param controller the controller that controls the output. - * @param measurementSource the measurement of the process variable - * @param goal the controller's goal - * @param useOutput the controller's output - * @param requirements the subsystems required by this command - */ - ProfiledPIDCommand(frc::ProfiledPIDController controller, - std::function measurementSource, - Distance_t goal, - std::function useOutput, - std::initializer_list requirements) + Requirements requirements = {}) : ProfiledPIDCommand( controller, measurementSource, [goal] { return goal; }, useOutput, requirements) {} @@ -193,7 +112,7 @@ class ProfiledPIDCommand std::function measurementSource, Distance_t goal, std::function useOutput, - std::span requirements = {}) + Requirements requirements = {}) : ProfiledPIDCommand( controller, measurementSource, [goal] { return goal; }, useOutput, requirements) {} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h index 055d8ec1876..1339bb15646 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h @@ -79,68 +79,7 @@ class RamseteCommand : public CommandHelper { frc::PIDController leftController, frc::PIDController rightController, std::function output, - std::initializer_list requirements); - - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. PID control and feedforward are handled internally, - * and outputs are scaled -12 to 12 representing units of volts. - * - *

Note: The controller will *not* set the outputVolts to zero upon - * completion of the path - this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of - * the odometry classes to provide this. - * @param controller The RAMSETE controller used to follow the - * trajectory. - * @param feedforward A component for calculating the feedforward for the - * drive. - * @param kinematics The kinematics for the robot drivetrain. - * @param wheelSpeeds A function that supplies the speeds of the left - * and right sides of the robot drive. - * @param leftController The PIDController for the left side of the robot - * drive. - * @param rightController The PIDController for the right side of the robot - * drive. - * @param output A function that consumes the computed left and right - * outputs (in volts) for the robot drive. - * @param requirements The subsystems to require. - */ - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::SimpleMotorFeedforward feedforward, - frc::DifferentialDriveKinematics kinematics, - std::function wheelSpeeds, - frc::PIDController leftController, - frc::PIDController rightController, - std::function output, - std::span requirements = {}); - - /** - * Constructs a new RamseteCommand that, when executed, will follow the - * provided trajectory. Performs no PID control and calculates no - * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller, - * and will need to be converted into a usable form by the user. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of - * the odometry classes to provide this. - * @param controller The RAMSETE controller used to follow the - * trajectory. - * @param kinematics The kinematics for the robot drivetrain. - * @param output A function that consumes the computed left and right - * wheel speeds. - * @param requirements The subsystems to require. - */ - RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, - frc::DifferentialDriveKinematics kinematics, - std::function - output, - std::initializer_list requirements); + Requirements requirements = {}); /** * Constructs a new RamseteCommand that, when executed, will follow the @@ -164,7 +103,7 @@ class RamseteCommand : public CommandHelper { std::function output, - std::span requirements = {}); + Requirements requirements = {}); void Initialize() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Requirements.h b/wpilibNewCommands/src/main/native/include/frc2/command/Requirements.h new file mode 100644 index 00000000000..968917f781a --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Requirements.h @@ -0,0 +1,46 @@ +// 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 "frc2/command/Subsystem.h" + +namespace frc2 { + +/** + * Represents requirements for a command, which is a set of (pointers to) + * subsystems. This class is implicitly convertible from std::initializer_list + * and std::span. + */ +class Requirements { + public: + // NOLINTNEXTLINE + /*implicit*/ Requirements(std::initializer_list requirements) + : m_subsystems{requirements.begin(), requirements.end()} {} + + // NOLINTNEXTLINE + /*implicit*/ Requirements(std::span requirements) + : m_subsystems{requirements.begin(), requirements.end()} {} + + Requirements() = default; + + Requirements(const Requirements&) = default; + + std::vector::const_iterator begin() const { + return m_subsystems.begin(); + } + + std::vector::const_iterator end() const { + return m_subsystems.end(); + } + + private: + std::vector m_subsystems; +}; + +} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h index 3bfecf7ffb6..021ba4009f0 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h @@ -22,16 +22,6 @@ namespace frc2 { */ class RunCommand : public CommandHelper { public: - /** - * Creates a new RunCommand. The Runnable will be run continuously until the - * command ends. Does not run when disabled. - * - * @param toRun the Runnable to run - * @param requirements the subsystems to require - */ - RunCommand(std::function toRun, - std::initializer_list requirements); - /** * Creates a new RunCommand. The Runnable will be run continuously until the * command ends. Does not run when disabled. @@ -40,7 +30,7 @@ class RunCommand : public CommandHelper { * @param requirements the subsystems to require */ explicit RunCommand(std::function toRun, - std::span requirements = {}); + Requirements requirements = {}); RunCommand(RunCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h index b1f56b2061b..20d890a10db 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h @@ -33,18 +33,7 @@ class StartEndCommand * @param requirements the subsystems required by this command */ StartEndCommand(std::function onInit, std::function onEnd, - std::initializer_list requirements); - - /** - * Creates a new StartEndCommand. Will run the given runnables when the - * command starts and when it ends. - * - * @param onInit the Runnable to run on command init - * @param onEnd the Runnable to run on command end - * @param requirements the subsystems required by this command - */ - StartEndCommand(std::function onInit, std::function onEnd, - std::span requirements = {}); + Requirements requirements = {}); StartEndCommand(StartEndCommand&& other) = default; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h index 96d0505c68b..835a0894216 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h @@ -94,7 +94,7 @@ class SwerveControllerCommand std::function desiredRotation, std::function)> output, - std::initializer_list requirements); + Requirements requirements = {}); /** * Constructs a new SwerveControllerCommand that when executed will follow the @@ -132,7 +132,7 @@ class SwerveControllerCommand frc::ProfiledPIDController thetaController, std::function)> output, - std::initializer_list requirements); + Requirements requirements = {}); /** * Constructs a new SwerveControllerCommand that when executed will follow the @@ -144,149 +144,11 @@ class SwerveControllerCommand * completion of the path- this is left to the user, since it is not * appropriate for paths with nonstationary endstates. * - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param desiredRotation The angle that the drivetrain should be - * facing. This is sampled at each time step. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::PIDController xController, frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> - output, - std::span requirements = {}); - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the - * provided trajectory. This command will not return output voltages but - * rather raw module states from the position controllers which need to be put - * into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path- this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller - * for the robot's x position. - * @param yController The Trajectory Tracker PID controller - * for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller - * for angle for the robot. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::PIDController xController, frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> - output, - std::span requirements = {}); - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the - * provided trajectory. This command will not return output voltages but - * rather raw module states from the position controllers which need to be put - * into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path- this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the drivetrain. - * @param desiredRotation The angle that the drivetrain should be - * facing. This is sampled at each time step. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> - output, - std::initializer_list requirements); - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the - * provided trajectory. This command will not return output voltages but - * rather raw module states from the position controllers which need to be put - * into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path- this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - *

Note 2: The final rotation of the robot will be set to the rotation of - * the final pose in the trajectory. The robot will not follow the rotations - * from the poses at each timestep. If alternate rotation behavior is desired, - * the other constructor with a supplier for rotation should be used. - * * @param trajectory The trajectory to follow. * @param pose A function that supplies the robot pose, * provided by the odometry class. * @param kinematics The kinematics for the robot drivetrain. * @param controller The HolonomicDriveController for the drivetrain. - * @param output The raw output module states from the - * position controllers. - * @param requirements The subsystems to require. - */ - SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> - output, - std::initializer_list requirements); - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the - * provided trajectory. This command will not return output voltages but - * rather raw module states from the position controllers which need to be put - * into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon - * completion of the path- this is left to the user, since it is not - * appropriate for paths with nonstationary endstates. - * - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose, - * provided by the odometry class. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the robot. * @param desiredRotation The angle that the drivetrain should be * facing. This is sampled at each time step. * @param output The raw output module states from the @@ -300,7 +162,7 @@ class SwerveControllerCommand std::function desiredRotation, std::function)> output, - std::span requirements = {}); + Requirements requirements = {}); /** * Constructs a new SwerveControllerCommand that when executed will follow the @@ -332,7 +194,7 @@ class SwerveControllerCommand frc::HolonomicDriveController controller, std::function)> output, - std::span requirements = {}); + Requirements requirements = {}); void Initialize() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc index 0b33429ac2c..d0c34af2b72 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -20,7 +20,7 @@ SwerveControllerCommand::SwerveControllerCommand( frc::ProfiledPIDController thetaController, std::function desiredRotation, std::function)> output, - std::initializer_list requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_kinematics(kinematics), @@ -37,7 +37,7 @@ SwerveControllerCommand::SwerveControllerCommand( frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController thetaController, std::function)> output, - std::initializer_list requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_kinematics(kinematics), @@ -46,72 +46,6 @@ SwerveControllerCommand::SwerveControllerCommand( this->AddRequirements(requirements); } -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::PIDController xController, frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> output, - std::span requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_desiredRotation(std::move(desiredRotation)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::PIDController xController, frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> output, - std::span requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(xController, yController, thetaController), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> output, - std::initializer_list requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(std::move(controller)), - m_desiredRotation(std::move(desiredRotation)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - -template -SwerveControllerCommand::SwerveControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> output, - std::initializer_list requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_kinematics(kinematics), - m_controller(std::move(controller)), - m_outputStates(output) { - this->AddRequirements(requirements); -} - template SwerveControllerCommand::SwerveControllerCommand( frc::Trajectory trajectory, std::function pose, @@ -119,7 +53,7 @@ SwerveControllerCommand::SwerveControllerCommand( frc::HolonomicDriveController controller, std::function desiredRotation, std::function)> output, - std::span requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_kinematics(kinematics), @@ -135,7 +69,7 @@ SwerveControllerCommand::SwerveControllerCommand( frc::SwerveDriveKinematics kinematics, frc::HolonomicDriveController controller, std::function)> output, - std::span requirements) + Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_kinematics(kinematics), diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index 56be97a497d..ccbb07b6615 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -47,7 +47,7 @@ class TrapezoidProfileCommand std::function output, std::function goal, std::function currentState, - std::initializer_list requirements) + Requirements requirements = {}) : m_profile(profile), m_output(output), m_goal(goal), @@ -56,50 +56,6 @@ class TrapezoidProfileCommand m_newAPI = true; } - /** - * Creates a new TrapezoidProfileCommand that will execute the given - * TrapezoidalProfile. Output will be piped to the provided consumer function. - * - * @param profile The motion profile to execute. - * @param output The consumer for the profile output. - * @param goal The supplier for the desired state - * @param currentState The current state - * @param requirements The list of requirements. - */ - TrapezoidProfileCommand(frc::TrapezoidProfile profile, - std::function output, - std::function goal, - std::function currentState, - std::span requirements = {}) - : m_profile(profile), - m_output(output), - m_goal(goal), - m_currentState(currentState) { - this->AddRequirements(requirements); - m_newAPI = true; - } - - /** - * Creates a new TrapezoidProfileCommand that will execute the given - * TrapezoidalProfile. Output will be piped to the provided consumer function. - * - * @param profile The motion profile to execute. - * @param output The consumer for the profile output. - * @param requirements The list of requirements. - * @deprecated The new constructor allows you to pass in a supplier for - * desired and current state. This allows you to change goals at runtime. - */ - WPI_DEPRECATED( - "The new constructor allows you to pass in a supplier for desired and " - "current state. This allows you to change goals at runtime.") - TrapezoidProfileCommand(frc::TrapezoidProfile profile, - std::function output, - std::initializer_list requirements) - : m_profile(profile), m_output(output) { - this->AddRequirements(requirements); - m_newAPI = false; - } - /** * Creates a new TrapezoidProfileCommand that will execute the given * TrapezoidalProfile. Output will be piped to the provided consumer function. @@ -115,7 +71,7 @@ class TrapezoidProfileCommand "current state. This allows you to change goals at runtime.") TrapezoidProfileCommand(frc::TrapezoidProfile profile, std::function output, - std::span requirements = {}) + Requirements requirements = {}) : m_profile(profile), m_output(output) { this->AddRequirements(requirements); m_newAPI = false; diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/AddRequirementsTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/AddRequirementsTest.cpp new file mode 100644 index 00000000000..8ef515474ea --- /dev/null +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/AddRequirementsTest.cpp @@ -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. + +#include + +#include "CommandTestBase.h" +#include "frc2/command/Command.h" +#include "frc2/command/RunCommand.h" + +using namespace frc2; + +// Class to verify the overload resolution of Command::AddRequirements. This +// does not derive from Command because AddRequirements is non-virtual, +// preventing overriding anyways. +class MockAddRequirements { + public: + MOCK_METHOD(void, AddRequirements, (Requirements), ()); + MOCK_METHOD(void, AddRequirements, ((wpi::SmallSet)), ()); + MOCK_METHOD(void, AddRequirements, (Subsystem*), ()); +}; + +TEST(AddRequirementsTest, InitializerListOverloadResolution) { + TestSubsystem requirement; + + MockAddRequirements overloadResolver; + + EXPECT_CALL(overloadResolver, AddRequirements(testing::An())); + + overloadResolver.AddRequirements({&requirement, &requirement}); +} + +TEST(AddRequirementsTest, SpanOverloadResolution) { + std::span requirementsSpan; + + MockAddRequirements overloadResolver; + + EXPECT_CALL(overloadResolver, AddRequirements(testing::An())); + + overloadResolver.AddRequirements(requirementsSpan); +} + +TEST(AddRequirementsTest, SmallSetOverloadResolution) { + wpi::SmallSet requirementsSet; + + MockAddRequirements overloadResolver; + + EXPECT_CALL(overloadResolver, + AddRequirements(testing::An>())); + + overloadResolver.AddRequirements(requirementsSet); +} + +TEST(AddRequirementsTest, SubsystemOverloadResolution) { + TestSubsystem requirement; + + MockAddRequirements overloadResolver; + + EXPECT_CALL(overloadResolver, AddRequirements(testing::An())); + + overloadResolver.AddRequirements(&requirement); +} + +TEST(AddRequirementsTest, InitializerListSemantics) { + TestSubsystem requirement1; + TestSubsystem requirement2; + + RunCommand command([] {}); + command.AddRequirements({&requirement1, &requirement2}); + EXPECT_TRUE(command.HasRequirement(&requirement1)); + EXPECT_TRUE(command.HasRequirement(&requirement2)); + EXPECT_EQ(command.GetRequirements().size(), 2u); +} + +TEST(AddRequirementsTest, InitializerListDuplicatesSemantics) { + TestSubsystem requirement; + + RunCommand command([] {}); + command.AddRequirements({&requirement, &requirement}); + EXPECT_TRUE(command.HasRequirement(&requirement)); + EXPECT_EQ(command.GetRequirements().size(), 1u); +} + +TEST(AddRequirementsTest, SpanSemantics) { + TestSubsystem requirement1; + TestSubsystem requirement2; + + wpi::array requirementsArray(&requirement1, + &requirement2); + + RunCommand command([] {}); + command.AddRequirements(std::span{requirementsArray}); + EXPECT_TRUE(command.HasRequirement(&requirement1)); + EXPECT_TRUE(command.HasRequirement(&requirement2)); + EXPECT_EQ(command.GetRequirements().size(), 2u); +} + +TEST(AddRequirementsTest, SpanDuplicatesSemantics) { + TestSubsystem requirement; + + wpi::array requirementsArray(&requirement, &requirement); + + RunCommand command([] {}); + command.AddRequirements(std::span{requirementsArray}); + EXPECT_TRUE(command.HasRequirement(&requirement)); + EXPECT_EQ(command.GetRequirements().size(), 1u); +} + +TEST(AddRequirementsTest, SmallSetSemantics) { + TestSubsystem requirement1; + TestSubsystem requirement2; + + wpi::SmallSet requirementsSet; + requirementsSet.insert(&requirement1); + requirementsSet.insert(&requirement2); + + RunCommand command([] {}); + command.AddRequirements(requirementsSet); + EXPECT_TRUE(command.HasRequirement(&requirement1)); + EXPECT_TRUE(command.HasRequirement(&requirement2)); + EXPECT_EQ(command.GetRequirements().size(), 2u); +} + +TEST(AddRequirementsTest, SubsystemPointerSemantics) { + TestSubsystem requirement1; + TestSubsystem requirement2; + + RunCommand command([] {}); + command.AddRequirements(&requirement1); + command.AddRequirements(&requirement2); + EXPECT_TRUE(command.HasRequirement(&requirement1)); + EXPECT_TRUE(command.HasRequirement(&requirement2)); + EXPECT_EQ(command.GetRequirements().size(), 2u); +} + +TEST(AddRequirementsTest, SubsystemPointerDuplicatesSemantics) { + TestSubsystem requirement; + + RunCommand command([] {}); + command.AddRequirements(&requirement); + command.AddRequirements(&requirement); + EXPECT_TRUE(command.HasRequirement(&requirement)); + EXPECT_EQ(command.GetRequirements().size(), 1u); +} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp index 75884a27464..0b6b8b00552 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp @@ -8,7 +8,7 @@ CloseClaw::CloseClaw(Claw& claw) : m_claw(&claw) { SetName("CloseClaw"); - AddRequirements({m_claw}); + AddRequirements(m_claw); } // Called just before this Command runs the first time diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp index e5bab77ea81..371ba85b995 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp @@ -9,7 +9,7 @@ OpenClaw::OpenClaw(Claw& claw) : frc2::CommandHelper(1_s), m_claw(&claw) { SetName("OpenClaw"); - AddRequirements({m_claw}); + AddRequirements(m_claw); } // Called just before this Command runs the first time diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp index 08343fec077..d8a2f08d4d3 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp @@ -11,7 +11,7 @@ SetElevatorSetpoint::SetElevatorSetpoint(double setpoint, Elevator& elevator) : m_setpoint(setpoint), m_elevator(&elevator) { SetName("SetElevatorSetpoint"); - AddRequirements({m_elevator}); + AddRequirements(m_elevator); } // Called just before this Command runs the first time diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp index 96977452236..bed2ffe86d8 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp @@ -9,7 +9,7 @@ SetWristSetpoint::SetWristSetpoint(double setpoint, Wrist& wrist) : m_setpoint(setpoint), m_wrist(&wrist) { SetName("SetWristSetpoint"); - AddRequirements({m_wrist}); + AddRequirements(m_wrist); } // Called just before this Command runs the first time diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp index cc34e535a97..7021f9d5fb7 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp @@ -14,7 +14,7 @@ TankDrive::TankDrive(std::function left, m_right(std::move(right)), m_drivetrain(&drivetrain) { SetName("TankDrive"); - AddRequirements({m_drivetrain}); + AddRequirements(m_drivetrain); } // Called repeatedly when this Command is scheduled to run diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp index 4dd9a765e46..6d1272d17ba 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp @@ -25,7 +25,7 @@ TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive) // reference m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value()); - AddRequirements({drive}); + AddRequirements(drive); } bool TurnToAngle::IsFinished() { diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp index df4e3554da9..464e0cf5cbe 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp @@ -30,7 +30,7 @@ TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target, // reference GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance); - AddRequirements({drive}); + AddRequirements(drive); } bool TurnToAngleProfiled::IsFinished() { diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp index 7ef404eb56d..4047596cf17 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp @@ -12,7 +12,7 @@ DefaultDrive::DefaultDrive(DriveSubsystem* subsystem, : m_drive{subsystem}, m_forward{std::move(forward)}, m_rotation{std::move(rotation)} { - AddRequirements({subsystem}); + AddRequirements(subsystem); } void DefaultDrive::Execute() { diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp index 7ee1b2c9305..9d9704d42c0 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp @@ -9,7 +9,7 @@ DriveDistance::DriveDistance(double inches, double speed, DriveSubsystem* subsystem) : m_drive(subsystem), m_distance(inches), m_speed(speed) { - AddRequirements({subsystem}); + AddRequirements(subsystem); } void DriveDistance::Initialize() { diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp index 7e6c9e8c2e4..12f36dafe85 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp @@ -5,7 +5,7 @@ #include "commands/ReleaseHatch.h" ReleaseHatch::ReleaseHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) { - AddRequirements({subsystem}); + AddRequirements(subsystem); } void ReleaseHatch::Initialize() { diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp index 0f585272ec0..e457af9c439 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp @@ -12,7 +12,7 @@ TeleopArcadeDrive::TeleopArcadeDrive( : m_drive{subsystem}, m_xaxisSpeedSupplier{xaxisSpeedSupplier}, m_zaxisRotateSupplier{zaxisRotateSuppplier} { - AddRequirements({subsystem}); + AddRequirements(subsystem); } void TeleopArcadeDrive::Execute() { diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h index cdfb74fb019..60115314010 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h @@ -14,7 +14,7 @@ class DriveDistance : public frc2::CommandHelper { public: DriveDistance(double speed, units::meter_t distance, Drivetrain* drive) : m_speed(speed), m_distance(distance), m_drive(drive) { - AddRequirements({m_drive}); + AddRequirements(m_drive); } void Initialize() override; diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h index 226c5258a82..9dfd240447e 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h @@ -15,7 +15,7 @@ class DriveTime : public frc2::CommandHelper { public: DriveTime(double speed, units::second_t time, Drivetrain* drive) : m_speed(speed), m_duration(time), m_drive(drive) { - AddRequirements({m_drive}); + AddRequirements(m_drive); } void Initialize() override; diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h index 6afc08a4561..c0f25b19002 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h @@ -15,7 +15,7 @@ class TurnDegrees : public frc2::CommandHelper { public: TurnDegrees(double speed, units::degree_t angle, Drivetrain* drive) : m_speed(speed), m_angle(angle), m_drive(drive) { - AddRequirements({m_drive}); + AddRequirements(m_drive); } void Initialize() override; diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h index c2c190ef55f..3c478a834db 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h @@ -15,7 +15,7 @@ class TurnTime : public frc2::CommandHelper { public: TurnTime(double speed, units::second_t time, Drivetrain* drive) : m_speed(speed), m_duration(time), m_drive(drive) { - AddRequirements({m_drive}); + AddRequirements(m_drive); } void Initialize() override;