diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index 91a565eb52..3a55ae82ce 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -35,96 +35,54 @@ void Command::Initialize() {} void Command::Execute() {} void Command::End(bool interrupted) {} -ParallelRaceGroup Command::WithTimeout(units::second_t duration) && { - std::vector> temp; - temp.emplace_back(std::make_unique(duration)); - temp.emplace_back(std::move(*this).TransferOwnership()); - return ParallelRaceGroup(std::move(temp)); +CommandPtr Command::WithTimeout(units::second_t duration) && { + return CommandPtr(std::move(*this).TransferOwnership()).WithTimeout(duration); } -ParallelRaceGroup Command::Until(std::function condition) && { - std::vector> temp; - temp.emplace_back(std::make_unique(std::move(condition))); - temp.emplace_back(std::move(*this).TransferOwnership()); - return ParallelRaceGroup(std::move(temp)); +CommandPtr Command::Until(std::function condition) && { + return CommandPtr(std::move(*this).TransferOwnership()) + .Until(std::move(condition)); } -std::unique_ptr Command::IgnoringDisable(bool doesRunWhenDisabled) && { - class RunsWhenDisabledCommand - : public CommandHelper { - public: - RunsWhenDisabledCommand(std::unique_ptr&& command, - bool doesRunWhenDisabled) - : CommandHelper(std::move(command)), - m_runsWhenDisabled(doesRunWhenDisabled) {} - bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } - - private: - bool m_runsWhenDisabled; - }; - - return std::make_unique( - std::move(*this).TransferOwnership(), doesRunWhenDisabled); +CommandPtr Command::IgnoringDisable(bool doesRunWhenDisabled) && { + return CommandPtr(std::move(*this).TransferOwnership()) + .IgnoringDisable(doesRunWhenDisabled); } -std::unique_ptr Command::WithInterruptBehavior( +CommandPtr Command::WithInterruptBehavior( InterruptionBehavior interruptBehavior) && { - class InterruptBehaviorCommand - : public CommandHelper { - public: - InterruptBehaviorCommand(std::unique_ptr&& command, - InterruptionBehavior interruptBehavior) - : CommandHelper(std::move(command)), - m_interruptBehavior(interruptBehavior) {} - InterruptionBehavior GetInterruptionBehavior() const override { - return m_interruptBehavior; - } - - private: - InterruptionBehavior m_interruptBehavior; - }; - - return std::make_unique( - std::move(*this).TransferOwnership(), interruptBehavior); + return CommandPtr(std::move(*this).TransferOwnership()) + .WithInterruptBehavior(interruptBehavior); } -ParallelRaceGroup Command::WithInterrupt(std::function condition) && { - std::vector> temp; - temp.emplace_back(std::make_unique(std::move(condition))); - temp.emplace_back(std::move(*this).TransferOwnership()); - return ParallelRaceGroup(std::move(temp)); +CommandPtr Command::WithInterrupt(std::function condition) && { + return CommandPtr(std::move(*this).TransferOwnership()) + .Until(std::move(condition)); } -SequentialCommandGroup Command::BeforeStarting( +CommandPtr Command::BeforeStarting( std::function toRun, std::initializer_list requirements) && { return std::move(*this).BeforeStarting( std::move(toRun), {requirements.begin(), requirements.end()}); } -SequentialCommandGroup Command::BeforeStarting( +CommandPtr Command::BeforeStarting( std::function toRun, wpi::span requirements) && { - std::vector> temp; - temp.emplace_back( - std::make_unique(std::move(toRun), requirements)); - temp.emplace_back(std::move(*this).TransferOwnership()); - return SequentialCommandGroup(std::move(temp)); + return CommandPtr(std::move(*this).TransferOwnership()) + .BeforeStarting(std::move(toRun), requirements); } -SequentialCommandGroup Command::AndThen( - std::function toRun, - std::initializer_list requirements) && { +CommandPtr Command::AndThen(std::function toRun, + std::initializer_list requirements) && { return std::move(*this).AndThen(std::move(toRun), {requirements.begin(), requirements.end()}); } -SequentialCommandGroup Command::AndThen( - std::function toRun, wpi::span requirements) && { - std::vector> temp; - temp.emplace_back(std::move(*this).TransferOwnership()); - temp.emplace_back( - std::make_unique(std::move(toRun), requirements)); - return SequentialCommandGroup(std::move(temp)); +CommandPtr Command::AndThen(std::function toRun, + wpi::span requirements) && { + return CommandPtr(std::move(*this).TransferOwnership()) + .AndThen(std::move(toRun), requirements); } PerpetualCommand Command::Perpetually() && { @@ -133,22 +91,21 @@ PerpetualCommand Command::Perpetually() && { WPI_UNIGNORE_DEPRECATED } -EndlessCommand Command::Endlessly() && { - return EndlessCommand(std::move(*this).TransferOwnership()); +CommandPtr Command::Endlessly() && { + return CommandPtr(std::move(*this).TransferOwnership()).Endlessly(); } -RepeatCommand Command::Repeatedly() && { - return RepeatCommand(std::move(*this).TransferOwnership()); +CommandPtr Command::Repeatedly() && { + return CommandPtr(std::move(*this).TransferOwnership()).Repeatedly(); } -ProxyScheduleCommand Command::AsProxy() { - return ProxyScheduleCommand(this); +CommandPtr Command::AsProxy() && { + return CommandPtr(std::move(*this).TransferOwnership()).AsProxy(); } -ConditionalCommand Command::Unless(std::function condition) && { - return ConditionalCommand(std::make_unique(), - std::move(*this).TransferOwnership(), - std::move(condition)); +CommandPtr Command::Unless(std::function condition) && { + return CommandPtr(std::move(*this).TransferOwnership()) + .Unless(std::move(condition)); } void Command::Schedule() { diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp new file mode 100644 index 0000000000..c7d54dd71a --- /dev/null +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp @@ -0,0 +1,156 @@ +// 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/CommandPtr.h" + +#include "frc2/command/CommandScheduler.h" +#include "frc2/command/ConditionalCommand.h" +#include "frc2/command/EndlessCommand.h" +#include "frc2/command/InstantCommand.h" +#include "frc2/command/ParallelCommandGroup.h" +#include "frc2/command/ParallelDeadlineGroup.h" +#include "frc2/command/ParallelRaceGroup.h" +#include "frc2/command/PerpetualCommand.h" +#include "frc2/command/ProxyScheduleCommand.h" +#include "frc2/command/RepeatCommand.h" +#include "frc2/command/SequentialCommandGroup.h" +#include "frc2/command/WaitCommand.h" +#include "frc2/command/WaitUntilCommand.h" +#include "frc2/command/WrapperCommand.h" + +using namespace frc2; + +CommandPtr CommandPtr::Repeatedly() && { + m_ptr = std::make_unique(std::move(m_ptr)); + return std::move(*this); +} + +CommandPtr CommandPtr::Endlessly() && { + m_ptr = std::make_unique(std::move(m_ptr)); + return std::move(*this); +} + +CommandPtr CommandPtr::AsProxy() && { + m_ptr = std::make_unique(std::move(m_ptr)); + return std::move(*this); +} + +class RunsWhenDisabledCommand : public WrapperCommand { + public: + RunsWhenDisabledCommand(std::unique_ptr&& command, + bool doesRunWhenDisabled) + : WrapperCommand(std::move(command)), + m_runsWhenDisabled(doesRunWhenDisabled) {} + + bool RunsWhenDisabled() const override { return m_runsWhenDisabled; } + + private: + bool m_runsWhenDisabled; +}; + +CommandPtr CommandPtr::IgnoringDisable(bool doesRunWhenDisabled) && { + m_ptr = std::make_unique(std::move(m_ptr), + doesRunWhenDisabled); + return std::move(*this); +} + +using InterruptionBehavior = Command::InterruptionBehavior; +class InterruptBehaviorCommand : public WrapperCommand { + public: + InterruptBehaviorCommand(std::unique_ptr&& command, + InterruptionBehavior interruptBehavior) + : WrapperCommand(std::move(command)), + m_interruptBehavior(interruptBehavior) {} + + InterruptionBehavior GetInterruptionBehavior() const override { + return m_interruptBehavior; + } + + private: + InterruptionBehavior m_interruptBehavior; +}; + +CommandPtr CommandPtr::WithInterruptBehavior( + InterruptionBehavior interruptBehavior) && { + m_ptr = std::make_unique(std::move(m_ptr), + interruptBehavior); + return std::move(*this); +} + +CommandPtr CommandPtr::AndThen(std::function toRun, + wpi::span requirements) && { + std::vector> temp; + temp.emplace_back(std::move(m_ptr)); + temp.emplace_back( + std::make_unique(std::move(toRun), requirements)); + m_ptr = std::make_unique(std::move(temp)); + return std::move(*this); +} + +CommandPtr CommandPtr::AndThen( + std::function toRun, + std::initializer_list requirements) && { + return std::move(*this).AndThen(std::move(toRun), + {requirements.begin(), requirements.end()}); +} + +CommandPtr CommandPtr::BeforeStarting( + std::function toRun, wpi::span requirements) && { + std::vector> temp; + temp.emplace_back( + std::make_unique(std::move(toRun), requirements)); + temp.emplace_back(std::move(m_ptr)); + m_ptr = std::make_unique(std::move(temp)); + return std::move(*this); +} + +CommandPtr CommandPtr::BeforeStarting( + std::function toRun, + std::initializer_list requirements) && { + return std::move(*this).BeforeStarting( + std::move(toRun), {requirements.begin(), requirements.end()}); +} + +CommandPtr CommandPtr::WithTimeout(units::second_t duration) && { + std::vector> temp; + temp.emplace_back(std::make_unique(duration)); + temp.emplace_back(std::move(m_ptr)); + m_ptr = std::make_unique(std::move(temp)); + return std::move(*this); +} + +CommandPtr CommandPtr::Until(std::function condition) && { + std::vector> temp; + temp.emplace_back(std::make_unique(std::move(condition))); + temp.emplace_back(std::move(m_ptr)); + m_ptr = std::make_unique(std::move(temp)); + return std::move(*this); +} + +CommandPtr CommandPtr::Unless(std::function condition) && { + m_ptr = std::make_unique( + std::make_unique(), std::move(m_ptr), + std::move(condition)); + return std::move(*this); +} + +Command* CommandPtr::get() const { + return m_ptr.get(); +} + +void CommandPtr::Schedule() const { + CommandScheduler::GetInstance().Schedule(*this); +} + +void CommandPtr::Cancel() const { + CommandScheduler::GetInstance().Cancel(*this); +} + +bool CommandPtr::IsScheduled() const { + return CommandScheduler::GetInstance().IsScheduled(*this); +} + +bool CommandPtr::HasRequirement(Subsystem* requirement) const { + return m_ptr->HasRequirement(requirement); +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 63fa6546bd..db34d49ccc 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -19,6 +19,7 @@ #include #include "frc2/command/CommandGroupBase.h" +#include "frc2/command/CommandPtr.h" #include "frc2/command/Subsystem.h" using namespace frc2; @@ -173,6 +174,10 @@ void CommandScheduler::Schedule(std::initializer_list commands) { } } +void CommandScheduler::Schedule(const CommandPtr& command) { + Schedule(command.get()); +} + void CommandScheduler::Run() { if (m_impl->disabled) { return; @@ -326,6 +331,10 @@ void CommandScheduler::Cancel(Command* command) { m_watchdog.AddEpoch(command->GetName() + ".End(true)"); } +void CommandScheduler::Cancel(const CommandPtr& command) { + Cancel(command.get()); +} + void CommandScheduler::Cancel(wpi::span commands) { for (auto command : commands) { Cancel(command); @@ -370,6 +379,10 @@ bool CommandScheduler::IsScheduled(const Command* command) const { return m_impl->scheduledCommands.contains(command); } +bool CommandScheduler::IsScheduled(const CommandPtr& command) const { + return m_impl->scheduledCommands.contains(command.get()); +} + Command* CommandScheduler::Requiring(const Subsystem* subsystem) const { auto find = m_impl->requirements.find(subsystem); if (find != m_impl->requirements.end()) { diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp index 373f0c6a7d..a0e617d375 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp @@ -13,6 +13,11 @@ Button Button::WhenPressed(Command* command) { return *this; } +Button Button::WhenPressed(CommandPtr&& command) { + WhenActive(std::move(command)); + return *this; +} + Button Button::WhenPressed(std::function toRun, std::initializer_list requirements) { WhenActive(std::move(toRun), requirements); @@ -30,6 +35,11 @@ Button Button::WhileHeld(Command* command) { return *this; } +Button Button::WhileHeld(CommandPtr&& command) { + WhileActiveContinous(std::move(command)); + return *this; +} + Button Button::WhileHeld(std::function toRun, std::initializer_list requirements) { WhileActiveContinous(std::move(toRun), requirements); @@ -47,11 +57,21 @@ Button Button::WhenHeld(Command* command) { return *this; } +Button Button::WhenHeld(CommandPtr&& command) { + WhileActiveOnce(std::move(command)); + return *this; +} + Button Button::WhenReleased(Command* command) { WhenInactive(command); return *this; } +Button Button::WhenReleased(CommandPtr&& command) { + WhenInactive(std::move(command)); + return *this; +} + Button Button::WhenReleased(std::function toRun, std::initializer_list requirements) { WhenInactive(std::move(toRun), requirements); @@ -69,6 +89,11 @@ Button Button::ToggleWhenPressed(Command* command) { return *this; } +Button Button::ToggleWhenPressed(CommandPtr&& command) { + ToggleWhenActive(std::move(command)); + return *this; +} + Button Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); return *this; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp index 40383a3c6b..bdd152041a 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -18,6 +18,11 @@ Trigger Trigger::WhenActive(Command* command) { return *this; } +Trigger Trigger::WhenActive(CommandPtr&& command) { + this->Rising().IfHigh([command = std::move(command)] { command.Schedule(); }); + return *this; +} + Trigger Trigger::WhenActive(std::function toRun, std::initializer_list requirements) { return WhenActive(std::move(toRun), @@ -35,6 +40,13 @@ Trigger Trigger::WhileActiveContinous(Command* command) { return *this; } +Trigger Trigger::WhileActiveContinous(CommandPtr&& command) { + auto ptr = std::make_shared(std::move(command)); + this->IfHigh([ptr] { ptr->Schedule(); }); + this->Falling().IfHigh([ptr] { ptr->Cancel(); }); + return *this; +} + Trigger Trigger::WhileActiveContinous( std::function toRun, std::initializer_list requirements) { @@ -53,11 +65,24 @@ Trigger Trigger::WhileActiveOnce(Command* command) { return *this; } +Trigger Trigger::WhileActiveOnce(CommandPtr&& command) { + auto ptr = std::make_shared(std::move(command)); + this->Rising().IfHigh([ptr] { ptr->Schedule(); }); + this->Falling().IfHigh([ptr] { ptr->Cancel(); }); + return *this; +} + Trigger Trigger::WhenInactive(Command* command) { this->Falling().IfHigh([command] { command->Schedule(); }); return *this; } +Trigger Trigger::WhenInactive(CommandPtr&& command) { + this->Falling().IfHigh( + [command = std::move(command)] { command.Schedule(); }); + return *this; +} + Trigger Trigger::WhenInactive(std::function toRun, std::initializer_list requirements) { return WhenInactive(std::move(toRun), @@ -80,6 +105,17 @@ Trigger Trigger::ToggleWhenActive(Command* command) { return *this; } +Trigger Trigger::ToggleWhenActive(CommandPtr&& command) { + this->Rising().IfHigh([command = std::move(command)] { + if (command.IsScheduled()) { + command.Cancel(); + } else { + command.Schedule(); + } + }); + return *this; +} + Trigger Trigger::CancelWhenActive(Command* command) { this->Rising().IfHigh([command] { command->Cancel(); }); return *this; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 94b62b1b18..72e983f0f8 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -24,15 +24,8 @@ std::string GetTypeName(const T& type) { return wpi::Demangle(typeid(type).name()); } -class EndlessCommand; -class ParallelCommandGroup; -class ParallelRaceGroup; -class ParallelDeadlineGroup; -class SequentialCommandGroup; class PerpetualCommand; class ProxyScheduleCommand; -class RepeatCommand; -class ConditionalCommand; /** * A state machine representing a complete action to be performed by the robot. @@ -121,6 +114,8 @@ class Command { kCancelIncoming }; + friend class CommandPtr; + /** * Decorates this command with a timeout. If the specified timeout is * exceeded before the command finishes normally, the command will be @@ -130,7 +125,7 @@ class Command { * @param duration the timeout duration * @return the command with the timeout added */ - ParallelRaceGroup WithTimeout(units::second_t duration) &&; + [[nodiscard]] CommandPtr WithTimeout(units::second_t duration) &&; /** * Decorates this command with an interrupt condition. If the specified @@ -141,7 +136,7 @@ class Command { * @param condition the interrupt condition * @return the command with the interrupt condition added */ - ParallelRaceGroup Until(std::function condition) &&; + [[nodiscard]] CommandPtr Until(std::function condition) &&; /** * Decorates this command with an interrupt condition. If the specified @@ -154,7 +149,7 @@ class Command { * @deprecated Replace with Until() */ WPI_DEPRECATED("Replace with Until()") - ParallelRaceGroup WithInterrupt(std::function condition) &&; + [[nodiscard]] CommandPtr WithInterrupt(std::function condition) &&; /** * Decorates this command with a runnable to run before this command starts. @@ -163,7 +158,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup BeforeStarting( + [[nodiscard]] CommandPtr BeforeStarting( std::function toRun, std::initializer_list requirements) &&; @@ -174,7 +169,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup BeforeStarting( + [[nodiscard]] CommandPtr BeforeStarting( std::function toRun, wpi::span requirements = {}) &&; @@ -185,7 +180,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup AndThen( + [[nodiscard]] CommandPtr AndThen( std::function toRun, std::initializer_list requirements) &&; @@ -196,7 +191,7 @@ class Command { * @param requirements the required subsystems * @return the decorated command */ - SequentialCommandGroup AndThen( + [[nodiscard]] CommandPtr AndThen( std::function toRun, wpi::span requirements = {}) &&; @@ -216,7 +211,7 @@ class Command { * * @return the decorated command */ - EndlessCommand Endlessly() &&; + [[nodiscard]] CommandPtr Endlessly() &&; /** * Decorates this command to run repeatedly, restarting it when it ends, until @@ -224,7 +219,7 @@ class Command { * * @return the decorated command */ - RepeatCommand Repeatedly() &&; + [[nodiscard]] CommandPtr Repeatedly() &&; /** * Decorates this command to run "by proxy" by wrapping it in a @@ -232,9 +227,11 @@ class Command { * when the user does not wish to extend the command's requirements to the * entire command group. * + *

This overload transfers command ownership to the returned CommandPtr. + * * @return the decorated command */ - ProxyScheduleCommand AsProxy(); + [[nodiscard]] CommandPtr AsProxy() &&; /** * Decorates this command to only run if this condition is not met. If the @@ -245,7 +242,7 @@ class Command { * @param condition the condition that will prevent the command from running * @return the decorated command */ - ConditionalCommand Unless(std::function condition) &&; + [[nodiscard]] CommandPtr Unless(std::function condition) &&; /** * Decorates this command to run or stop when disabled. @@ -253,7 +250,7 @@ class Command { * @param doesRunWhenDisabled true to run when disabled. * @return the decorated command */ - std::unique_ptr IgnoringDisable(bool doesRunWhenDisabled) &&; + [[nodiscard]] CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&; /** * Decorates this command to run or stop when disabled. @@ -261,8 +258,8 @@ class Command { * @param interruptBehavior true to run when disabled. * @return the decorated command */ - std::unique_ptr WithInterruptBehavior( - InterruptionBehavior interruptBehavior) &&; + [[nodiscard]] CommandPtr WithInterruptBehavior( + Command::InterruptionBehavior interruptBehavior) &&; /** * Schedules this command. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h index 3c5fd7b6ef..0ff55add80 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h @@ -9,6 +9,7 @@ #include #include "frc2/command/Command.h" +#include "frc2/command/CommandPtr.h" namespace frc2 { @@ -28,6 +29,11 @@ class CommandHelper : public Base { public: CommandHelper() = default; + CommandPtr ToPtr() && { + return CommandPtr( + std::make_unique(std::move(*static_cast(this)))); + } + protected: std::unique_ptr TransferOwnership() && override { return std::make_unique(std::move(*static_cast(this))); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h new file mode 100644 index 0000000000..2f05ff2e21 --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h @@ -0,0 +1,189 @@ +// 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 "frc2/command/Command.h" + +namespace frc2 { +class CommandPtr final { + public: + explicit CommandPtr(std::unique_ptr&& command) + : m_ptr(std::move(command)) {} + + template >>> + explicit CommandPtr(T&& command) + : CommandPtr(std::make_unique>( + std::forward(command))) {} + + CommandPtr(CommandPtr&&) = default; + CommandPtr& operator=(CommandPtr&&) = default; + + /** + * Decorates this command to run repeatedly, restarting it when it ends, until + * this command is interrupted. The decorated command can still be canceled. + * + * @return the decorated command + */ + [[nodiscard]] CommandPtr Repeatedly() &&; + + /** + * Decorates this command to run endlessly, ignoring its ordinary end + * conditions. The decorated command can still be interrupted or canceled. + * + * @return the decorated command + */ + [[nodiscard]] CommandPtr Endlessly() &&; + + /** + * Decorates this command to run "by proxy" by wrapping it in a + * ProxyScheduleCommand. This is useful for "forking off" from command groups + * when the user does not wish to extend the command's requirements to the + * entire command group. + * + * @return the decorated command + */ + [[nodiscard]] CommandPtr AsProxy() &&; + + /** + * Decorates this command to run or stop when disabled. + * + * @param doesRunWhenDisabled true to run when disabled. + * @return the decorated command + */ + [[nodiscard]] CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&; + + /** + * Decorates this command to run or stop when disabled. + * + * @param interruptBehavior true to run when disabled. + * @return the decorated command + */ + [[nodiscard]] CommandPtr WithInterruptBehavior( + Command::InterruptionBehavior interruptBehavior) &&; + + /** + * 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, + wpi::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) &&; + + /** + * 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::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, + wpi::span requirements = {}) &&; + + /** + * Decorates this command with a timeout. If the specified timeout is + * exceeded before the command finishes normally, the command will be + * interrupted and un-scheduled. Note that the timeout only applies to the + * command returned by this method; the calling command is not itself changed. + * + * @param duration the timeout duration + * @return the command with the timeout added + */ + [[nodiscard]] CommandPtr WithTimeout(units::second_t duration) &&; + + /** + * Decorates this command with an interrupt condition. If the specified + * condition becomes true before the command finishes normally, the command + * will be interrupted and un-scheduled. Note that this only applies to the + * command returned by this method; the calling command is not itself changed. + * + * @param condition the interrupt condition + * @return the command with the interrupt condition added + */ + [[nodiscard]] CommandPtr Until(std::function condition) &&; + + /** + * Decorates this command to only run if this condition is not met. If the + * command is already running and the condition changes to true, the command + * will not stop running. The requirements of this command will be kept for + * the new conditonal command. + * + * @param condition the condition that will prevent the command from running + * @return the decorated command + */ + [[nodiscard]] CommandPtr Unless(std::function condition) &&; + + /** + * Get a raw pointer to the held command. + */ + Command* get() const; + + /** + * Schedules this command. + */ + void Schedule() const; + + /** + * Cancels this command. Will call End(true). Commands will be canceled + * regardless of interruption behavior. + */ + void Cancel() const; + + /** + * Whether or not the command is currently scheduled. Note that this does not + * detect whether the command is being run by a CommandGroup, only whether it + * is directly being run by the scheduler. + * + * @return Whether the command is scheduled. + */ + bool IsScheduled() const; + + /** + * Whether the command requires a given subsystem. Named "HasRequirement" + * rather than "requires" to avoid confusion with Command::Requires(Subsystem) + * -- this may be able to be changed in a few years. + * + * @param requirement the subsystem to inquire about + * @return whether the subsystem is required + */ + bool HasRequirement(Subsystem* requirement) const; + + private: + std::unique_ptr m_ptr; +}; + +} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index 75fc3ef2cc..067734a96f 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -20,6 +20,7 @@ namespace frc2 { class Command; +class CommandPtr; class Subsystem; /** @@ -82,6 +83,17 @@ class CommandScheduler final : public nt::NTSendable, WPI_DEPRECATED("Call Clear on the EventLoop instance directly!") void ClearButtons(); + /** + * Schedules a command for execution. Does nothing if the command is already + * scheduled. If a command's requirements are not available, it will only be + * started if all the commands currently using those requirements are + * interruptible. If this is the case, they will be interrupted and the + * command will be scheduled. + * + * @param command the command to schedule + */ + void Schedule(const CommandPtr& command); + /** * Schedules a command for execution. Does nothing if the command is already * scheduled. If a command's requirements are not available, it will only be @@ -200,6 +212,18 @@ class CommandScheduler final : public nt::NTSendable, */ void Cancel(Command* command); + /** + * Cancels commands. The scheduler will only call Command::End() + * method of the canceled command with true, indicating they were + * canceled (as opposed to finishing normally). + * + *

Commands will be canceled even if they are not scheduled as + * interruptible. + * + * @param command the command to cancel + */ + void Cancel(const CommandPtr& command); + /** * Cancels commands. The scheduler will only call Command::End() * method of the canceled command with true, indicating they were @@ -259,6 +283,16 @@ class CommandScheduler final : public nt::NTSendable, */ bool IsScheduled(const Command* command) const; + /** + * Whether a given command is running. Note that this only works on commands + * that are directly scheduled by the scheduler; it will not work on commands + * inside of CommandGroups, as the scheduler does not see them. + * + * @param command the command to query + * @return whether the command is currently scheduled + */ + bool IsScheduled(const CommandPtr& command) const; + /** * Returns the command currently requiring a given subsystem. Null if no * command is currently requiring the subsystem diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h index bbd71966e4..44c6d959c2 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h @@ -11,6 +11,7 @@ #include #include "Trigger.h" +#include "frc2/command/CommandPtr.h" namespace frc2 { class Command; @@ -47,6 +48,16 @@ class Button : public Trigger { */ Button WhenPressed(Command* command); + /** + * Binds a command to start when the button is pressed. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Button WhenPressed(CommandPtr&& command); + /** * Binds a command to start when the button is pressed. Transfers * command ownership to the button scheduler, so the user does not have to @@ -91,6 +102,16 @@ class Button : public Trigger { */ Button WhileHeld(Command* command); + /** + * Binds a command to be started repeatedly while the button is pressed, and + * canceled when it is released. Transfers command ownership to the button + * scheduler, so the user does not have to worry about lifespan. + * + * @param command The command to bind. + * @return The button, for chained calls. + */ + Button WhileHeld(CommandPtr&& command); + /** * Binds a command to be started repeatedly while the button is pressed, and * canceled when it is released. Transfers command ownership to the button @@ -135,6 +156,16 @@ class Button : public Trigger { */ Button WhenHeld(Command* command); + /** + * Binds a command to be started when the button is pressed, and canceled + * when it is released. Transfers command ownership to the button scheduler, + * so the user does not have to worry about lifespan. + * + * @param command The command to bind. + * @return The button, for chained calls. + */ + Button WhenHeld(CommandPtr&& command); + /** * Binds a command to be started when the button is pressed, and canceled * when it is released. Transfers command ownership to the button scheduler, @@ -161,6 +192,16 @@ class Button : public Trigger { */ Button WhenReleased(Command* command); + /** + * Binds a command to start when the button is pressed. Transfers + * command ownership to the button scheduler, so the user does not have to + * worry about lifespan. + * + * @param command The command to bind. + * @return The button, for chained calls. + */ + Button WhenReleased(CommandPtr&& command); + /** * Binds a command to start when the button is pressed. Transfers * command ownership to the button scheduler, so the user does not have to @@ -205,6 +246,16 @@ class Button : public Trigger { */ Button ToggleWhenPressed(Command* command); + /** + * Binds a command to start when the button is pressed, and be canceled when + * it is pessed again. Transfers command ownership to the button scheduler, + * so the user does not have to worry about lifespan. + * + * @param command The command to bind. + * @return The button, for chained calls. + */ + Button ToggleWhenPressed(CommandPtr&& command); + /** * Binds a command to start when the button is pressed, and be canceled when * it is pessed again. Transfers command ownership to the button scheduler, diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index 13fb10bd02..c098ad47c5 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h @@ -69,6 +69,15 @@ class Trigger : public frc::BooleanEvent { */ Trigger WhenActive(Command* command); + /** + * Binds a command to start when the trigger becomes active. Moves + * command ownership to the button scheduler. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger WhenActive(CommandPtr&& command); + /** * Binds a command to start when the trigger becomes active. Transfers * command ownership to the button scheduler, so the user does not have to @@ -116,6 +125,16 @@ class Trigger : public frc::BooleanEvent { */ Trigger WhileActiveContinous(Command* command); + /** + * Binds a command to be started repeatedly while the trigger is active, and + * canceled when it becomes inactive. Moves command ownership to the button + * scheduler. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger WhileActiveContinous(CommandPtr&& command); + /** * Binds a command to be started repeatedly while the trigger is active, and * canceled when it becomes inactive. Transfers command ownership to the @@ -164,6 +183,16 @@ class Trigger : public frc::BooleanEvent { */ Trigger WhileActiveOnce(Command* command); + /** + * Binds a command to be started when the trigger becomes active, and + * canceled when it becomes inactive. Moves command ownership to the button + * scheduler. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger WhileActiveOnce(CommandPtr&& command); + /** * Binds a command to be started when the trigger becomes active, and * canceled when it becomes inactive. Transfers command ownership to the @@ -195,6 +224,15 @@ class Trigger : public frc::BooleanEvent { */ Trigger WhenInactive(Command* command); + /** + * Binds a command to start when the trigger becomes inactive. Moves + * command ownership to the button scheduler. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger WhenInactive(CommandPtr&& command); + /** * Binds a command to start when the trigger becomes inactive. Transfers * command ownership to the button scheduler, so the user does not have to @@ -242,6 +280,16 @@ class Trigger : public frc::BooleanEvent { */ Trigger ToggleWhenActive(Command* command); + /** + * Binds a command to start when the trigger becomes active, and be canceled + * when it again becomes active. Moves command ownership to the button + * scheduler. + * + * @param command The command to bind. + * @return The trigger, for chained calls. + */ + Trigger ToggleWhenActive(CommandPtr&& command); + /** * Binds a command to start when the trigger becomes active, and be canceled * when it again becomes active. Transfers command ownership to the button diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp index 22f5fe263e..76e9cb39d7 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -23,15 +23,15 @@ TEST_F(CommandDecoratorTest, WithTimeout) { auto command = RunCommand([] {}, {}).WithTimeout(100_ms); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); frc::sim::StepTiming(150_ms); scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); frc::sim::ResumeTiming(); } @@ -43,15 +43,15 @@ TEST_F(CommandDecoratorTest, Until) { auto command = RunCommand([] {}, {}).Until([&finished] { return finished; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); finished = true; scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); } TEST_F(CommandDecoratorTest, IgnoringDisable) { @@ -61,10 +61,10 @@ TEST_F(CommandDecoratorTest, IgnoringDisable) { SetDSEnabled(false); - scheduler.Schedule(command.get()); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(command.get())); + EXPECT_TRUE(scheduler.IsScheduled(command)); } TEST_F(CommandDecoratorTest, BeforeStarting) { @@ -75,14 +75,14 @@ TEST_F(CommandDecoratorTest, BeforeStarting) { auto command = InstantCommand([] {}, {}).BeforeStarting( [&finished] { finished = true; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); EXPECT_TRUE(finished); scheduler.Run(); scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); } TEST_F(CommandDecoratorTest, AndThen) { @@ -93,14 +93,14 @@ TEST_F(CommandDecoratorTest, AndThen) { auto command = InstantCommand([] {}, {}).AndThen([&finished] { finished = true; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); EXPECT_FALSE(finished); scheduler.Run(); scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); EXPECT_TRUE(finished); } @@ -124,12 +124,12 @@ TEST_F(CommandDecoratorTest, Endlessly) { auto command = InstantCommand([] {}, {}).Endlessly(); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); } TEST_F(CommandDecoratorTest, Unless) { @@ -143,12 +143,12 @@ TEST_F(CommandDecoratorTest, Unless) { return unlessBool; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); EXPECT_FALSE(hasRun); unlessBool = false; - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); EXPECT_TRUE(hasRun); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp index 00958b32f0..79a472fc50 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp @@ -54,7 +54,7 @@ TEST_F(CommandRequirementsTest, RequirementUninterruptible) { int exeCounter = 0; int endCounter = 0; - std::unique_ptr command1 = + CommandPtr command1 = FunctionalCommand([&initCounter] { initCounter++; }, [&exeCounter] { exeCounter++; }, [&endCounter](bool interruptible) { endCounter++; }, @@ -68,13 +68,13 @@ TEST_F(CommandRequirementsTest, RequirementUninterruptible) { EXPECT_CALL(command2, End(true)).Times(0); EXPECT_CALL(command2, End(false)).Times(0); - scheduler.Schedule(command1.get()); + scheduler.Schedule(command1); EXPECT_EQ(1, initCounter); scheduler.Run(); EXPECT_EQ(1, exeCounter); - EXPECT_TRUE(scheduler.IsScheduled(command1.get())); + EXPECT_TRUE(scheduler.IsScheduled(command1)); scheduler.Schedule(&command2); - EXPECT_TRUE(scheduler.IsScheduled(command1.get())); + EXPECT_TRUE(scheduler.IsScheduled(command1)); EXPECT_FALSE(scheduler.IsScheduled(&command2)); scheduler.Run(); EXPECT_EQ(2, exeCounter); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/EndlessCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/EndlessCommandTest.cpp index 21099ae201..0cf017b078 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/EndlessCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/EndlessCommandTest.cpp @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. #include "CommandTestBase.h" -#include "frc2/command/EndlessCommand.h" #include "frc2/command/InstantCommand.h" using namespace frc2; @@ -14,10 +13,10 @@ TEST_F(EndlessCommandTest, EndlessCommandSchedule) { bool check = false; - EndlessCommand command{InstantCommand([&check] { check = true; }, {})}; + auto command = InstantCommand([&check] { check = true; }, {}).Endlessly(); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); EXPECT_TRUE(check); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp index a932eaa537..9c66769990 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp @@ -5,6 +5,7 @@ #include #include "CommandTestBase.h" +#include "frc2/command/CommandPtr.h" #include "frc2/command/InstantCommand.h" #include "frc2/command/ProxyScheduleCommand.h" #include "frc2/command/WaitUntilCommand.h" @@ -51,10 +52,10 @@ TEST_F(ProxyScheduleCommandTest, OwningCommandSchedule) { bool scheduled = false; - ProxyScheduleCommand command( - std::make_unique([&scheduled] { scheduled = true; })); + CommandPtr command = + InstantCommand([&scheduled] { scheduled = true; }).AsProxy(); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); EXPECT_TRUE(scheduled); @@ -65,15 +66,15 @@ TEST_F(ProxyScheduleCommandTest, OwningCommandEnd) { bool finished = false; - ProxyScheduleCommand command( - std::make_unique([&finished] { return finished; })); + CommandPtr command = + WaitUntilCommand([&finished] { return finished; }).AsProxy(); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); finished = true; scheduler.Run(); scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp index 0ec5bc806b..ee933c21e0 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp @@ -4,7 +4,6 @@ #include "CommandTestBase.h" #include "frc2/command/FunctionalCommand.h" -#include "frc2/command/RepeatCommand.h" using namespace frc2; class RepeatCommandTest : public CommandTestBase {}; @@ -33,7 +32,7 @@ TEST_F(RepeatCommandTest, CallsMethodsCorrectly) { EXPECT_EQ(0, isFinishedCounter); EXPECT_EQ(0, endCounter); - scheduler.Schedule(&command); + scheduler.Schedule(command); EXPECT_EQ(1, initCounter); EXPECT_EQ(0, exeCounter); EXPECT_EQ(0, isFinishedCounter); diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp index c459739b96..9743a4df58 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp @@ -47,5 +47,5 @@ void RobotContainer::ConfigureButtonBindings() { frc2::Command* RobotContainer::GetAutonomousCommand() { // Runs the chosen command in autonomous - return &m_autonomousCommand; + return m_autonomousCommand.get(); } diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h index 26714fe643..4c97c006da 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h @@ -45,7 +45,7 @@ class RobotContainer { ShooterSubsystem m_shooter; // A simple autonomous routine that shoots the loaded frisbees - frc2::SequentialCommandGroup m_autonomousCommand = + frc2::CommandPtr m_autonomousCommand = frc2::SequentialCommandGroup{ // Start the command by spinning up the shooter... frc2::InstantCommand([this] { m_shooter.Enable(); }, {&m_shooter}),