mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
Split the two command implementations into separate libraries (#2012)
This will allow us at the user code side to determine to include old commands, new commands or both.
This commit is contained in:
Thad House
committed by
Peter Johnson
Peter Johnson
parent
2ad15cae19
commit
509819d83f
242
wpilibNewCommands/src/main/native/include/frc2/command/Command.h
Normal file
242
wpilibNewCommands/src/main/native/include/frc2/command/Command.h
Normal file
@@ -0,0 +1,242 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/ErrorBase.h>
|
||||
#include <frc/WPIErrors.h>
|
||||
#include <frc2/command/Subsystem.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <units/units.h>
|
||||
#include <wpi/ArrayRef.h>
|
||||
#include <wpi/Demangle.h>
|
||||
#include <wpi/SmallSet.h>
|
||||
#include <wpi/Twine.h>
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
template <typename T>
|
||||
std::string GetTypeName(const T& type) {
|
||||
return wpi::Demangle(typeid(type).name());
|
||||
}
|
||||
|
||||
class ParallelCommandGroup;
|
||||
class ParallelRaceGroup;
|
||||
class ParallelDeadlineGroup;
|
||||
class SequentialCommandGroup;
|
||||
class PerpetualCommand;
|
||||
class ProxyScheduleCommand;
|
||||
|
||||
/**
|
||||
* A state machine representing a complete action to be performed by the robot.
|
||||
* Commands are run by the CommandScheduler, and can be composed into
|
||||
* CommandGroups to allow users to build complicated multi-step actions without
|
||||
* the need to roll the state machine logic themselves.
|
||||
*
|
||||
* <p>Commands are run synchronously from the main robot loop; no multithreading
|
||||
* is used, unless specified explicitly from the command implementation.
|
||||
*
|
||||
* <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
|
||||
* or decorators will not function!
|
||||
*
|
||||
* @see CommandScheduler
|
||||
* @see CommandHelper
|
||||
*/
|
||||
class Command : public frc::ErrorBase {
|
||||
public:
|
||||
Command() = default;
|
||||
virtual ~Command();
|
||||
|
||||
Command(const Command&);
|
||||
Command& operator=(const Command&);
|
||||
Command(Command&&) = default;
|
||||
Command& operator=(Command&&) = default;
|
||||
|
||||
/**
|
||||
* The initial subroutine of a command. Called once when the command is
|
||||
* initially scheduled.
|
||||
*/
|
||||
virtual void Initialize();
|
||||
|
||||
/**
|
||||
* The main body of a command. Called repeatedly while the command is
|
||||
* scheduled.
|
||||
*/
|
||||
virtual void Execute();
|
||||
|
||||
/**
|
||||
* The action to take when the command ends. Called when either the command
|
||||
* finishes normally, or when it interrupted/canceled.
|
||||
*
|
||||
* @param interrupted whether the command was interrupted/canceled
|
||||
*/
|
||||
virtual void End(bool interrupted);
|
||||
|
||||
/**
|
||||
* Whether the command has finished. Once a command finishes, the scheduler
|
||||
* will call its end() method and un-schedule it.
|
||||
*
|
||||
* @return whether the command has finished.
|
||||
*/
|
||||
virtual bool IsFinished() { return false; }
|
||||
|
||||
/**
|
||||
* Specifies the set of subsystems used by this command. Two commands cannot
|
||||
* use the same subsystem at the same time. If the command is scheduled as
|
||||
* interruptible and another command is scheduled that shares a requirement,
|
||||
* the command will be interrupted. Else, the command will not be scheduled.
|
||||
* If no subsystems are required, return an empty set.
|
||||
*
|
||||
* <p>Note: it is recommended that user implementations contain the
|
||||
* requirements as a field, and return that field here, rather than allocating
|
||||
* a new set every time this is called.
|
||||
*
|
||||
* @return the set of subsystems that are required
|
||||
*/
|
||||
virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
ParallelRaceGroup 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
|
||||
*/
|
||||
ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
|
||||
|
||||
/**
|
||||
* Decorates this command with a runnable to run before this command starts.
|
||||
*
|
||||
* @param toRun the Runnable to run
|
||||
* @return the decorated command
|
||||
*/
|
||||
SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
|
||||
|
||||
/**
|
||||
* Decorates this command with a runnable to run after the command finishes.
|
||||
*
|
||||
* @param toRun the Runnable to run
|
||||
* @return the decorated command
|
||||
*/
|
||||
SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
|
||||
|
||||
/**
|
||||
* Decorates this command to run perpetually, ignoring its ordinary end
|
||||
* conditions. The decorated command can still be interrupted or canceled.
|
||||
*
|
||||
* @return the decorated command
|
||||
*/
|
||||
PerpetualCommand Perpetually() &&;
|
||||
|
||||
/**
|
||||
* Decorates this command to run "by proxy" by wrapping it in a {@link
|
||||
* 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
|
||||
*/
|
||||
ProxyScheduleCommand AsProxy();
|
||||
|
||||
/**
|
||||
* Schedules this command.
|
||||
*
|
||||
* @param interruptible whether this command can be interrupted by another
|
||||
* command that shares one of its requirements
|
||||
*/
|
||||
void Schedule(bool interruptible);
|
||||
|
||||
/**
|
||||
* Schedules this command, defaulting to interruptible.
|
||||
*/
|
||||
void Schedule() { Schedule(true); }
|
||||
|
||||
/**
|
||||
* Cancels this command. Will call the command's interrupted() method.
|
||||
* Commands will be canceled even if they are not marked as interruptible.
|
||||
*/
|
||||
void Cancel();
|
||||
|
||||
/**
|
||||
* 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
|
||||
* {@link
|
||||
* edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.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;
|
||||
|
||||
/**
|
||||
* Whether the command is currently grouped in a command group. Used as extra
|
||||
* insurance to prevent accidental independent use of grouped commands.
|
||||
*/
|
||||
bool IsGrouped() const;
|
||||
|
||||
/**
|
||||
* Sets whether the command is currently grouped in a command group. Can be
|
||||
* used to "reclaim" a command if a group is no longer going to use it. NOT
|
||||
* ADVISED!
|
||||
*/
|
||||
void SetGrouped(bool grouped);
|
||||
|
||||
/**
|
||||
* Whether the given command should run when the robot is disabled. Override
|
||||
* to return true if the command should run when disabled.
|
||||
*
|
||||
* @return whether the command should run when the robot is disabled
|
||||
*/
|
||||
virtual bool RunsWhenDisabled() const { return false; }
|
||||
|
||||
virtual std::string GetName() const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Transfers ownership of this command to a unique pointer. Used for
|
||||
* decorator methods.
|
||||
*/
|
||||
virtual std::unique_ptr<Command> TransferOwnership() && = 0;
|
||||
|
||||
bool m_isGrouped = false;
|
||||
};
|
||||
|
||||
/**
|
||||
* Checks if two commands have disjoint requirement sets.
|
||||
*
|
||||
* @param first The first command to check.
|
||||
* @param second The second command to check.
|
||||
* @return False if first and second share a requirement.
|
||||
*/
|
||||
bool RequirementsDisjoint(Command* first, Command* second);
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,73 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/smartdashboard/Sendable.h>
|
||||
#include <frc/smartdashboard/SendableHelper.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <wpi/SmallSet.h>
|
||||
#include <wpi/Twine.h>
|
||||
|
||||
#include "Command.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A Sendable base class for Commands.
|
||||
*/
|
||||
class CommandBase : public Command,
|
||||
public frc::Sendable,
|
||||
public frc::SendableHelper<CommandBase> {
|
||||
public:
|
||||
/**
|
||||
* Adds the specified requirements to the command.
|
||||
*
|
||||
* @param requirements the requirements to add
|
||||
*/
|
||||
void AddRequirements(std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
|
||||
|
||||
wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
|
||||
|
||||
/**
|
||||
* Sets the name of this Command.
|
||||
*
|
||||
* @param name name
|
||||
*/
|
||||
void SetName(const wpi::Twine& name);
|
||||
|
||||
/**
|
||||
* Gets the name of this Command.
|
||||
*
|
||||
* @return Name
|
||||
*/
|
||||
std::string GetName() const override;
|
||||
|
||||
/**
|
||||
* Gets the subsystem name of this Command.
|
||||
*
|
||||
* @return Subsystem name
|
||||
*/
|
||||
std::string GetSubsystem() const;
|
||||
|
||||
/**
|
||||
* Sets the subsystem name of this Command.
|
||||
*
|
||||
* @param subsystem subsystem name
|
||||
*/
|
||||
void SetSubsystem(const wpi::Twine& subsystem);
|
||||
|
||||
void InitSendable(frc::SendableBuilder& builder) override;
|
||||
|
||||
protected:
|
||||
CommandBase();
|
||||
wpi::SmallSet<Subsystem*, 4> m_requirements;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,62 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/ErrorBase.h>
|
||||
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
#include "CommandBase.h"
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
/**
|
||||
* A base for CommandGroups. Statically tracks commands that have been
|
||||
* allocated to groups to ensure those commands are not also used independently,
|
||||
* which can result in inconsistent command state and unpredictable execution.
|
||||
*/
|
||||
class CommandGroupBase : public CommandBase {
|
||||
public:
|
||||
/**
|
||||
* Requires that the specified command not have been already allocated to a
|
||||
* CommandGroup. Reports an error if the command is already grouped.
|
||||
*
|
||||
* @param commands The command to check
|
||||
* @return True if all the command is ungrouped.
|
||||
*/
|
||||
static bool RequireUngrouped(Command& command);
|
||||
|
||||
/**
|
||||
* Requires that the specified commands not have been already allocated to a
|
||||
* CommandGroup. Reports an error if any of the commands are already grouped.
|
||||
*
|
||||
* @param commands The commands to check
|
||||
* @return True if all the commands are ungrouped.
|
||||
*/
|
||||
static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
|
||||
|
||||
/**
|
||||
* Requires that the specified commands not have been already allocated to a
|
||||
* CommandGroup. Reports an error if any of the commands are already grouped.
|
||||
*
|
||||
* @param commands The commands to check
|
||||
* @return True if all the commands are ungrouped.
|
||||
*/
|
||||
static bool RequireUngrouped(std::initializer_list<Command*>);
|
||||
|
||||
/**
|
||||
* Adds the given commands to the command group.
|
||||
*
|
||||
* @param commands The commands to add.
|
||||
*/
|
||||
virtual void AddCommands(
|
||||
std::vector<std::unique_ptr<Command>>&& commands) = 0;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,37 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include "Command.h"
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
/**
|
||||
* CRTP implementation to allow polymorphic decorator functions in Command.
|
||||
*
|
||||
* <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
|
||||
* or decorators will not function!
|
||||
*/
|
||||
template <typename Base, typename CRTP,
|
||||
typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
|
||||
class CommandHelper : public Base {
|
||||
using Base::Base;
|
||||
|
||||
public:
|
||||
CommandHelper() = default;
|
||||
|
||||
protected:
|
||||
std::unique_ptr<Command> TransferOwnership() && override {
|
||||
return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
|
||||
}
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,372 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/ErrorBase.h>
|
||||
#include <frc/RobotState.h>
|
||||
#include <frc/WPIErrors.h>
|
||||
#include <frc/smartdashboard/Sendable.h>
|
||||
#include <frc/smartdashboard/SendableHelper.h>
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
#include <networktables/NetworkTableEntry.h>
|
||||
#include <wpi/DenseMap.h>
|
||||
#include <wpi/FunctionExtras.h>
|
||||
#include <wpi/SmallSet.h>
|
||||
|
||||
#include "CommandState.h"
|
||||
|
||||
namespace frc2 {
|
||||
class Command;
|
||||
class Subsystem;
|
||||
|
||||
/**
|
||||
* The scheduler responsible for running Commands. A Command-based robot should
|
||||
* call Run() on the singleton instance in its periodic block in order to run
|
||||
* commands synchronously from the main loop. Subsystems should be registered
|
||||
* with the scheduler using RegisterSubsystem() in order for their Periodic()
|
||||
* methods to be called and for their default commands to be scheduled.
|
||||
*/
|
||||
class CommandScheduler final : public frc::Sendable,
|
||||
public frc::ErrorBase,
|
||||
public frc::SendableHelper<CommandScheduler> {
|
||||
public:
|
||||
/**
|
||||
* Returns the Scheduler instance.
|
||||
*
|
||||
* @return the instance
|
||||
*/
|
||||
static CommandScheduler& GetInstance();
|
||||
|
||||
using Action = std::function<void(const Command&)>;
|
||||
|
||||
/**
|
||||
* Adds a button binding to the scheduler, which will be polled to schedule
|
||||
* commands.
|
||||
*
|
||||
* @param button The button to add
|
||||
*/
|
||||
void AddButton(wpi::unique_function<void()> button);
|
||||
|
||||
/**
|
||||
* Removes all button bindings from the scheduler.
|
||||
*/
|
||||
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 have been
|
||||
* scheduled as interruptible. If this is the case, they will be interrupted
|
||||
* and the command will be scheduled.
|
||||
*
|
||||
* @param interruptible whether this command can be interrupted
|
||||
* @param command the command to schedule
|
||||
*/
|
||||
void Schedule(bool interruptible, Command* command);
|
||||
|
||||
/**
|
||||
* Schedules a command for execution, with interruptible defaulted to true.
|
||||
* Does nothing if the command is already scheduled.
|
||||
*
|
||||
* @param command the command to schedule
|
||||
*/
|
||||
void Schedule(Command* command);
|
||||
|
||||
/**
|
||||
* Schedules multiple commands 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 have
|
||||
* been scheduled as interruptible. If this is the case, they will be
|
||||
* interrupted and the command will be scheduled.
|
||||
*
|
||||
* @param interruptible whether the commands should be interruptible
|
||||
* @param commands the commands to schedule
|
||||
*/
|
||||
void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
|
||||
|
||||
/**
|
||||
* Schedules multiple commands 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 have
|
||||
* been scheduled as interruptible. If this is the case, they will be
|
||||
* interrupted and the command will be scheduled.
|
||||
*
|
||||
* @param interruptible whether the commands should be interruptible
|
||||
* @param commands the commands to schedule
|
||||
*/
|
||||
void Schedule(bool interruptible, std::initializer_list<Command*> commands);
|
||||
|
||||
/**
|
||||
* Schedules multiple commands for execution, with interruptible defaulted to
|
||||
* true. Does nothing if the command is already scheduled.
|
||||
*
|
||||
* @param commands the commands to schedule
|
||||
*/
|
||||
void Schedule(wpi::ArrayRef<Command*> commands);
|
||||
|
||||
/**
|
||||
* Schedules multiple commands for execution, with interruptible defaulted to
|
||||
* true. Does nothing if the command is already scheduled.
|
||||
*
|
||||
* @param commands the commands to schedule
|
||||
*/
|
||||
void Schedule(std::initializer_list<Command*> commands);
|
||||
|
||||
/**
|
||||
* Runs a single iteration of the scheduler. The execution occurs in the
|
||||
* following order:
|
||||
*
|
||||
* <p>Subsystem periodic methods are called.
|
||||
*
|
||||
* <p>Button bindings are polled, and new commands are scheduled from them.
|
||||
*
|
||||
* <p>Currently-scheduled commands are executed.
|
||||
*
|
||||
* <p>End conditions are checked on currently-scheduled commands, and commands
|
||||
* that are finished have their end methods called and are removed.
|
||||
*
|
||||
* <p>Any subsystems not being used as requirements have their default methods
|
||||
* started.
|
||||
*/
|
||||
void Run();
|
||||
|
||||
/**
|
||||
* Registers subsystems with the scheduler. This must be called for the
|
||||
* subsystem's periodic block to run when the scheduler is run, and for the
|
||||
* subsystem's default command to be scheduled. It is recommended to call
|
||||
* this from the constructor of your subsystem implementations.
|
||||
*
|
||||
* @param subsystem the subsystem to register
|
||||
*/
|
||||
void RegisterSubsystem(Subsystem* subsystem);
|
||||
|
||||
/**
|
||||
* Un-registers subsystems with the scheduler. The subsystem will no longer
|
||||
* have its periodic block called, and will not have its default command
|
||||
* scheduled.
|
||||
*
|
||||
* @param subsystem the subsystem to un-register
|
||||
*/
|
||||
void UnregisterSubsystem(Subsystem* subsystem);
|
||||
|
||||
void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
|
||||
|
||||
void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
|
||||
|
||||
/**
|
||||
* Sets the default command for a subsystem. Registers that subsystem if it
|
||||
* is not already registered. Default commands will run whenever there is no
|
||||
* other command currently scheduled that requires the subsystem. Default
|
||||
* commands should be written to never end (i.e. their IsFinished() method
|
||||
* should return false), as they would simply be re-scheduled if they do.
|
||||
* Default commands must also require their subsystem.
|
||||
*
|
||||
* @param subsystem the subsystem whose default command will be set
|
||||
* @param defaultCommand the default command to associate with the subsystem
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
|
||||
if (!defaultCommand.HasRequirement(subsystem)) {
|
||||
wpi_setWPIErrorWithContext(
|
||||
CommandIllegalUse, "Default commands must require their subsystem!");
|
||||
return;
|
||||
}
|
||||
if (defaultCommand.IsFinished()) {
|
||||
wpi_setWPIErrorWithContext(CommandIllegalUse,
|
||||
"Default commands should not end!");
|
||||
return;
|
||||
}
|
||||
m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(defaultCommand));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the default command associated with this subsystem. Null if this
|
||||
* subsystem has no default command associated with it.
|
||||
*
|
||||
* @param subsystem the subsystem to inquire about
|
||||
* @return the default command associated with the subsystem
|
||||
*/
|
||||
Command* GetDefaultCommand(const Subsystem* subsystem) const;
|
||||
|
||||
/**
|
||||
* Cancels a command. The scheduler will only call the interrupted method of
|
||||
* a canceled command, not the end method (though the interrupted method may
|
||||
* itself call the end method). Commands will be canceled even if they are
|
||||
* not scheduled as interruptible.
|
||||
*
|
||||
* @param command the command to cancel
|
||||
*/
|
||||
void Cancel(Command* command);
|
||||
|
||||
/**
|
||||
* Cancels commands. The scheduler will only call the interrupted method of a
|
||||
* canceled command, not the end method (though the interrupted method may
|
||||
* itself call the end method). Commands will be canceled even if they are
|
||||
* not scheduled as interruptible.
|
||||
*
|
||||
* @param commands the commands to cancel
|
||||
*/
|
||||
void Cancel(wpi::ArrayRef<Command*> commands);
|
||||
|
||||
/**
|
||||
* Cancels commands. The scheduler will only call the interrupted method of a
|
||||
* canceled command, not the end method (though the interrupted method may
|
||||
* itself call the end method). Commands will be canceled even if they are
|
||||
* not scheduled as interruptible.
|
||||
*
|
||||
* @param commands the commands to cancel
|
||||
*/
|
||||
void Cancel(std::initializer_list<Command*> commands);
|
||||
|
||||
/**
|
||||
* Cancels all commands that are currently scheduled.
|
||||
*/
|
||||
void CancelAll();
|
||||
|
||||
/**
|
||||
* Returns the time since a given command was scheduled. 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 the time since the command was scheduled, in seconds
|
||||
*/
|
||||
double TimeSinceScheduled(const Command* command) const;
|
||||
|
||||
/**
|
||||
* Whether the given commands are 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 commands the command to query
|
||||
* @return whether the command is currently scheduled
|
||||
*/
|
||||
bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
|
||||
|
||||
/**
|
||||
* Whether the given commands are 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 commands the command to query
|
||||
* @return whether the command is currently scheduled
|
||||
*/
|
||||
bool IsScheduled(std::initializer_list<const Command*> commands) 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 commands the command to query
|
||||
* @return whether the command is currently scheduled
|
||||
*/
|
||||
bool IsScheduled(const Command* command) const;
|
||||
|
||||
/**
|
||||
* Returns the command currently requiring a given subsystem. Null if no
|
||||
* command is currently requiring the subsystem
|
||||
*
|
||||
* @param subsystem the subsystem to be inquired about
|
||||
* @return the command currently requiring the subsystem
|
||||
*/
|
||||
Command* Requiring(const Subsystem* subsystem) const;
|
||||
|
||||
/**
|
||||
* Disables the command scheduler.
|
||||
*/
|
||||
void Disable();
|
||||
|
||||
/**
|
||||
* Enables the command scheduler.
|
||||
*/
|
||||
void Enable();
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the initialization of any command by the
|
||||
* scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
void OnCommandInitialize(Action action);
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the execution of any command by the scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
void OnCommandExecute(Action action);
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the interruption of any command by the
|
||||
* scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
void OnCommandInterrupt(Action action);
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the finishing of any command by the scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
void OnCommandFinish(Action action);
|
||||
|
||||
void InitSendable(frc::SendableBuilder& builder) override;
|
||||
|
||||
private:
|
||||
// Constructor; private as this is a singleton
|
||||
CommandScheduler();
|
||||
|
||||
// A map from commands to their scheduling state. Also used as a set of the
|
||||
// currently-running commands.
|
||||
wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
|
||||
|
||||
// A map from required subsystems to their requiring commands. Also used as a
|
||||
// set of the currently-required subsystems.
|
||||
wpi::DenseMap<Subsystem*, Command*> m_requirements;
|
||||
|
||||
// A map from subsystems registered with the scheduler to their default
|
||||
// commands. Also used as a list of currently-registered subsystems.
|
||||
wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
|
||||
|
||||
// The set of currently-registered buttons that will be polled every
|
||||
// iteration.
|
||||
wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
|
||||
|
||||
bool m_disabled{false};
|
||||
|
||||
// NetworkTable entries for use in Sendable impl
|
||||
nt::NetworkTableEntry m_namesEntry;
|
||||
nt::NetworkTableEntry m_idsEntry;
|
||||
nt::NetworkTableEntry m_cancelEntry;
|
||||
|
||||
// Lists of user-supplied actions to be executed on scheduling events for
|
||||
// every command.
|
||||
wpi::SmallVector<Action, 4> m_initActions;
|
||||
wpi::SmallVector<Action, 4> m_executeActions;
|
||||
wpi::SmallVector<Action, 4> m_interruptActions;
|
||||
wpi::SmallVector<Action, 4> m_finishActions;
|
||||
|
||||
// Flag and queues for avoiding concurrent modification if commands are
|
||||
// scheduled/canceled during run
|
||||
|
||||
bool m_inRunLoop = false;
|
||||
wpi::DenseMap<Command*, bool> m_toSchedule;
|
||||
wpi::SmallVector<Command*, 4> m_toCancel;
|
||||
|
||||
friend class CommandTestBase;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,33 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* Class that holds scheduling state for a command. Used internally by the
|
||||
* CommandScheduler
|
||||
*/
|
||||
class CommandState final {
|
||||
public:
|
||||
CommandState() = default;
|
||||
|
||||
explicit CommandState(bool interruptible);
|
||||
|
||||
bool IsInterruptible() const { return m_interruptible; }
|
||||
|
||||
// The time since this command was initialized.
|
||||
double TimeSinceInitialized() const;
|
||||
|
||||
private:
|
||||
double m_startTime = -1;
|
||||
bool m_interruptible;
|
||||
|
||||
void StartTiming();
|
||||
void StartRunning();
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,92 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandGroupBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* Runs one of two commands, depending on the value of the given condition when
|
||||
* this command is initialized. Does not actually schedule the selected command
|
||||
* - rather, the command is run through this command; this ensures that the
|
||||
* command will behave as expected if used as part of a CommandGroup. Requires
|
||||
* the requirements of both commands, again to ensure proper functioning when
|
||||
* used in a CommandGroup. If this is undesired, consider using
|
||||
* ScheduleCommand.
|
||||
*
|
||||
* <p>As this command contains multiple component commands within it, it is
|
||||
* technically a command group; the command instances that are passed to it
|
||||
* cannot be added to any other groups, or scheduled individually.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*
|
||||
* @see ScheduleCommand
|
||||
*/
|
||||
class ConditionalCommand
|
||||
: public CommandHelper<CommandBase, ConditionalCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new ConditionalCommand.
|
||||
*
|
||||
* @param onTrue the command to run if the condition is true
|
||||
* @param onFalse the command to run if the condition is false
|
||||
* @param condition the condition to determine which command to run
|
||||
*/
|
||||
template <class T1, class T2,
|
||||
typename = std::enable_if_t<
|
||||
std::is_base_of_v<Command, std::remove_reference_t<T1>>>,
|
||||
typename = std::enable_if_t<
|
||||
std::is_base_of_v<Command, std::remove_reference_t<T2>>>>
|
||||
ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
|
||||
: ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
|
||||
std::forward<T1>(onTrue)),
|
||||
std::make_unique<std::remove_reference_t<T2>>(
|
||||
std::forward<T2>(onFalse)),
|
||||
condition) {}
|
||||
|
||||
/**
|
||||
* Creates a new ConditionalCommand.
|
||||
*
|
||||
* @param onTrue the command to run if the condition is true
|
||||
* @param onFalse the command to run if the condition is false
|
||||
* @param condition the condition to determine which command to run
|
||||
*/
|
||||
ConditionalCommand(std::unique_ptr<Command>&& onTrue,
|
||||
std::unique_ptr<Command>&& onFalse,
|
||||
std::function<bool()> condition);
|
||||
|
||||
ConditionalCommand(ConditionalCommand&& other) = default;
|
||||
|
||||
// No copy constructors for command groups
|
||||
ConditionalCommand(const ConditionalCommand& other) = delete;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
std::unique_ptr<Command> m_onTrue;
|
||||
std::unique_ptr<Command> m_onFalse;
|
||||
std::function<bool()> m_condition;
|
||||
Command* m_selectedCommand{nullptr};
|
||||
bool m_runsWhenDisabled = true;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,56 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that allows the user to pass in functions for each of the basic
|
||||
* command methods through the constructor. Useful for inline definitions of
|
||||
* complex commands - note, however, that if a command is beyond a certain
|
||||
* complexity it is usually better practice to write a proper class for it than
|
||||
* to inline it.
|
||||
*/
|
||||
class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
|
||||
public:
|
||||
/**
|
||||
* 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<void()> onInit,
|
||||
std::function<void()> onExecute,
|
||||
std::function<void(bool)> onEnd,
|
||||
std::function<bool()> isFinished);
|
||||
|
||||
FunctionalCommand(FunctionalCommand&& other) = default;
|
||||
|
||||
FunctionalCommand(const FunctionalCommand& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
std::function<void()> m_onInit;
|
||||
std::function<void()> m_onExecute;
|
||||
std::function<void(bool)> m_onEnd;
|
||||
std::function<bool()> m_isFinished;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,48 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A Command that runs instantly; it will initialize, execute once, and end on
|
||||
* the same iteration of the scheduler. Users can either pass in a Runnable and
|
||||
* a set of requirements, or else subclass this command if desired.
|
||||
*/
|
||||
class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
|
||||
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<void()> toRun,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
InstantCommand(InstantCommand&& other) = default;
|
||||
|
||||
InstantCommand(const InstantCommand& other) = default;
|
||||
|
||||
/**
|
||||
* Creates a new InstantCommand with a Runnable that does nothing. Useful
|
||||
* only as a no-arg constructor to call implicitly from subclass constructors.
|
||||
*/
|
||||
InstantCommand();
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
bool IsFinished() final;
|
||||
|
||||
private:
|
||||
std::function<void()> m_toRun;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,53 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Notifier.h>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that starts a notifier to run the given runnable periodically in a
|
||||
* separate thread. Has no end condition as-is; either subclass it or use {@link
|
||||
* Command#withTimeout(double)} or
|
||||
* {@link Command#withInterrupt(BooleanSupplier)} to give it one.
|
||||
*
|
||||
* <p>WARNING: Do not use this class unless you are confident in your ability to
|
||||
* make the executed code thread-safe. If you do not know what "thread-safe"
|
||||
* means, that is a good sign that you should not use this class.
|
||||
*/
|
||||
class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
|
||||
public:
|
||||
/**
|
||||
* 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<void()> toRun, units::second_t period,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
NotifierCommand(NotifierCommand&& other);
|
||||
|
||||
NotifierCommand(const NotifierCommand& other);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
private:
|
||||
std::function<void()> m_toRun;
|
||||
frc::Notifier m_notifier;
|
||||
units::second_t m_period;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,79 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc2/command/CommandBase.h"
|
||||
#include "frc2/command/CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that controls an output with a PIDController. Runs forever by
|
||||
* default - to add exit conditions and/or other behavior, subclass this class.
|
||||
* The controller calculation and output are performed synchronously in the
|
||||
* command's execute() method.
|
||||
*
|
||||
* @see PIDController
|
||||
*/
|
||||
class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
|
||||
public:
|
||||
/**
|
||||
* 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(PIDController controller,
|
||||
std::function<double()> measurementSource,
|
||||
std::function<double()> setpointSource,
|
||||
std::function<void(double)> useOutput,
|
||||
std::initializer_list<Subsystem*> 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(PIDController controller,
|
||||
std::function<double()> measurementSource, double setpoint,
|
||||
std::function<void(double)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
PIDCommand(PIDCommand&& other) = default;
|
||||
|
||||
PIDCommand(const PIDCommand& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
/**
|
||||
* Returns the PIDController used by the command.
|
||||
*
|
||||
* @return The PIDController
|
||||
*/
|
||||
PIDController& getController();
|
||||
|
||||
protected:
|
||||
PIDController m_controller;
|
||||
std::function<double()> m_measurement;
|
||||
std::function<double()> m_setpoint;
|
||||
std::function<void(double)> m_useOutput;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,73 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc2/command/SubsystemBase.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A subsystem that uses a PIDController to control an output. The controller
|
||||
* is run synchronously from the subsystem's periodic() method.
|
||||
*
|
||||
* @see PIDController
|
||||
*/
|
||||
class PIDSubsystem : public SubsystemBase {
|
||||
public:
|
||||
/**
|
||||
* Creates a new PIDSubsystem.
|
||||
*
|
||||
* @param controller the PIDController to use
|
||||
*/
|
||||
explicit PIDSubsystem(PIDController controller);
|
||||
|
||||
void Periodic() override;
|
||||
|
||||
/**
|
||||
* Uses the output from the PIDController.
|
||||
*
|
||||
* @param output the output of the PIDController
|
||||
*/
|
||||
virtual void UseOutput(double output) = 0;
|
||||
|
||||
/**
|
||||
* Returns the reference (setpoint) used by the PIDController.
|
||||
*
|
||||
* @return the reference (setpoint) to be used by the controller
|
||||
*/
|
||||
virtual double GetSetpoint() = 0;
|
||||
|
||||
/**
|
||||
* Returns the measurement of the process variable used by the PIDController.
|
||||
*
|
||||
* @return the measurement of the process variable
|
||||
*/
|
||||
virtual double GetMeasurement() = 0;
|
||||
|
||||
/**
|
||||
* Enables the PID control. Resets the controller.
|
||||
*/
|
||||
virtual void Enable();
|
||||
|
||||
/**
|
||||
* Disables the PID control. Sets output to zero.
|
||||
*/
|
||||
virtual void Disable();
|
||||
|
||||
/**
|
||||
* Returns the PIDController.
|
||||
*
|
||||
* @return The controller.
|
||||
*/
|
||||
PIDController& GetController();
|
||||
|
||||
protected:
|
||||
PIDController m_controller;
|
||||
bool m_enabled;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,100 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable : 4521)
|
||||
#endif
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "CommandGroupBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A CommandGroup that runs a set of commands in parallel, ending when the last
|
||||
* command ends.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*/
|
||||
class ParallelCommandGroup
|
||||
: public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new ParallelCommandGroup. The given commands will be executed
|
||||
* simultaneously. The command group will finish when the last command
|
||||
* finishes. If the CommandGroup is interrupted, only the commands that are
|
||||
* still running will be interrupted.
|
||||
*
|
||||
* @param commands the commands to include in this group.
|
||||
*/
|
||||
explicit ParallelCommandGroup(
|
||||
std::vector<std::unique_ptr<Command>>&& commands);
|
||||
|
||||
/**
|
||||
* Creates a new ParallelCommandGroup. The given commands will be executed
|
||||
* simultaneously. The command group will finish when the last command
|
||||
* finishes. If the CommandGroup is interrupted, only the commands that are
|
||||
* still running will be interrupted.
|
||||
*
|
||||
* @param commands the commands to include in this group.
|
||||
*/
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
explicit ParallelCommandGroup(Types&&... commands) {
|
||||
AddCommands(std::forward<Types>(commands)...);
|
||||
}
|
||||
|
||||
ParallelCommandGroup(ParallelCommandGroup&& other) = default;
|
||||
|
||||
// No copy constructors for commandgroups
|
||||
ParallelCommandGroup(const ParallelCommandGroup&) = delete;
|
||||
|
||||
// Prevent template expansion from emulating copy ctor
|
||||
ParallelCommandGroup(ParallelCommandGroup&) = delete;
|
||||
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
void AddCommands(Types&&... commands) {
|
||||
std::vector<std::unique_ptr<Command>> foo;
|
||||
((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
|
||||
std::forward<Types>(commands))),
|
||||
...);
|
||||
AddCommands(std::move(foo));
|
||||
}
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
|
||||
|
||||
std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
|
||||
bool m_runWhenDisabled{true};
|
||||
bool isRunning = false;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
@@ -0,0 +1,111 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable : 4521)
|
||||
#endif
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "CommandGroupBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A CommandGroup that runs a set of commands in parallel, ending only when a
|
||||
* specific command (the "deadline") ends, interrupting all other commands that
|
||||
* are still running at that point.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*/
|
||||
class ParallelDeadlineGroup
|
||||
: public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new ParallelDeadlineGroup. The given commands (including the
|
||||
* deadline) will be executed simultaneously. The CommandGroup will finish
|
||||
* when the deadline finishes, interrupting all other still-running commands.
|
||||
* If the CommandGroup is interrupted, only the commands still running will be
|
||||
* interrupted.
|
||||
*
|
||||
* @param deadline the command that determines when the group ends
|
||||
* @param commands the commands to be executed
|
||||
*/
|
||||
ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
|
||||
std::vector<std::unique_ptr<Command>>&& commands);
|
||||
/**
|
||||
* Creates a new ParallelDeadlineGroup. The given commands (including the
|
||||
* deadline) will be executed simultaneously. The CommandGroup will finish
|
||||
* when the deadline finishes, interrupting all other still-running commands.
|
||||
* If the CommandGroup is interrupted, only the commands still running will be
|
||||
* interrupted.
|
||||
*
|
||||
* @param deadline the command that determines when the group ends
|
||||
* @param commands the commands to be executed
|
||||
*/
|
||||
template <class T, class... Types,
|
||||
typename = std::enable_if_t<
|
||||
std::is_base_of_v<Command, std::remove_reference_t<T>>>,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
|
||||
SetDeadline(std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(deadline)));
|
||||
AddCommands(std::forward<Types>(commands)...);
|
||||
}
|
||||
|
||||
ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
|
||||
|
||||
// No copy constructors for command groups
|
||||
ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
|
||||
|
||||
// Prevent template expansion from emulating copy ctor
|
||||
ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
|
||||
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
void AddCommands(Types&&... commands) {
|
||||
std::vector<std::unique_ptr<Command>> foo;
|
||||
((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
|
||||
std::forward<Types>(commands))),
|
||||
...);
|
||||
AddCommands(std::move(foo));
|
||||
}
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
|
||||
|
||||
void SetDeadline(std::unique_ptr<Command>&& deadline);
|
||||
|
||||
std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
|
||||
Command* m_deadline;
|
||||
bool m_runWhenDisabled{true};
|
||||
bool isRunning = false;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
@@ -0,0 +1,90 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable : 4521)
|
||||
#endif
|
||||
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "CommandGroupBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A CommandGroup that runs a set of commands in parallel, ending when any one
|
||||
* of the commands ends and interrupting all the others.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*/
|
||||
class ParallelRaceGroup
|
||||
: public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new ParallelCommandRace. The given commands will be executed
|
||||
* simultaneously, and will "race to the finish" - the first command to finish
|
||||
* ends the entire command, with all other commands being interrupted.
|
||||
*
|
||||
* @param commands the commands to include in this group.
|
||||
*/
|
||||
explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
|
||||
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
explicit ParallelRaceGroup(Types&&... commands) {
|
||||
AddCommands(std::forward<Types>(commands)...);
|
||||
}
|
||||
|
||||
ParallelRaceGroup(ParallelRaceGroup&& other) = default;
|
||||
|
||||
// No copy constructors for command groups
|
||||
ParallelRaceGroup(const ParallelRaceGroup&) = delete;
|
||||
|
||||
// Prevent template expansion from emulating copy ctor
|
||||
ParallelRaceGroup(ParallelRaceGroup&) = delete;
|
||||
|
||||
template <class... Types>
|
||||
void AddCommands(Types&&... commands) {
|
||||
std::vector<std::unique_ptr<Command>> foo;
|
||||
((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
|
||||
std::forward<Types>(commands))),
|
||||
...);
|
||||
AddCommands(std::move(foo));
|
||||
}
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
|
||||
|
||||
std::set<std::unique_ptr<Command>> m_commands;
|
||||
bool m_runWhenDisabled{true};
|
||||
bool m_finished{false};
|
||||
bool isRunning = false;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
@@ -0,0 +1,78 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable : 4521)
|
||||
#endif
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandGroupBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that runs another command in perpetuity, ignoring that command's
|
||||
* end conditions. While this class does not extend {@link CommandGroupBase},
|
||||
* it is still considered a CommandGroup, as it allows one to compose another
|
||||
* command within it; the command instances that are passed to it cannot be
|
||||
* added to any other groups, or scheduled individually.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*/
|
||||
class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new PerpetualCommand. Will run another command in perpetuity,
|
||||
* ignoring that command's end conditions, unless this command itself is
|
||||
* interrupted.
|
||||
*
|
||||
* @param command the command to run perpetually
|
||||
*/
|
||||
explicit PerpetualCommand(std::unique_ptr<Command>&& command);
|
||||
|
||||
/**
|
||||
* Creates a new PerpetualCommand. Will run another command in perpetuity,
|
||||
* ignoring that command's end conditions, unless this command itself is
|
||||
* interrupted.
|
||||
*
|
||||
* @param command the command to run perpetually
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
explicit PerpetualCommand(T&& command)
|
||||
: PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(command))) {}
|
||||
|
||||
PerpetualCommand(PerpetualCommand&& other) = default;
|
||||
|
||||
// No copy constructors for command groups
|
||||
PerpetualCommand(const PerpetualCommand& other) = delete;
|
||||
|
||||
// Prevent template expansion from emulating copy ctor
|
||||
PerpetualCommand(PerpetualCommand&) = delete;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
private:
|
||||
std::unique_ptr<Command> m_command;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
@@ -0,0 +1,35 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/Twine.h>
|
||||
#include <wpi/raw_ostream.h>
|
||||
|
||||
#include "CommandHelper.h"
|
||||
#include "InstantCommand.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that prints a string when initialized.
|
||||
*/
|
||||
class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new a PrintCommand.
|
||||
*
|
||||
* @param message the message to print
|
||||
*/
|
||||
explicit PrintCommand(const wpi::Twine& message);
|
||||
|
||||
PrintCommand(PrintCommand&& other) = default;
|
||||
|
||||
PrintCommand(const PrintCommand& other) = default;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,116 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "frc/controller/ProfiledPIDController.h"
|
||||
#include "frc2/command/CommandBase.h"
|
||||
#include "frc2/command/CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that controls an output with a ProfiledPIDController. Runs forever
|
||||
* by default - to add exit conditions and/or other behavior, subclass this
|
||||
* class. The controller calculation and output are performed synchronously in
|
||||
* the command's execute() method.
|
||||
*
|
||||
* @see ProfiledPIDController
|
||||
*/
|
||||
class ProfiledPIDCommand
|
||||
: public CommandHelper<CommandBase, ProfiledPIDCommand> {
|
||||
using State = frc::TrapezoidProfile::State;
|
||||
|
||||
public:
|
||||
/**
|
||||
* 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<units::meter_t()> measurementSource,
|
||||
std::function<State()> goalSource,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> 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<units::meter_t()> measurementSource,
|
||||
std::function<units::meter_t()> goalSource,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> 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<units::meter_t()> measurementSource,
|
||||
State goal, std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> 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<units::meter_t()> measurementSource,
|
||||
units::meter_t goal,
|
||||
std::function<void(double, State)> useOutput,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
|
||||
|
||||
ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
/**
|
||||
* Returns the ProfiledPIDController used by the command.
|
||||
*
|
||||
* @return The ProfiledPIDController
|
||||
*/
|
||||
frc::ProfiledPIDController& GetController();
|
||||
|
||||
protected:
|
||||
frc::ProfiledPIDController m_controller;
|
||||
std::function<units::meter_t()> m_measurement;
|
||||
std::function<State()> m_goal;
|
||||
std::function<void(double, State)> m_useOutput;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,78 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "frc/controller/ProfiledPIDController.h"
|
||||
#include "frc2/command/SubsystemBase.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A subsystem that uses a ProfiledPIDController to control an output. The
|
||||
* controller is run synchronously from the subsystem's periodic() method.
|
||||
*
|
||||
* @see ProfiledPIDController
|
||||
*/
|
||||
class ProfiledPIDSubsystem : public SubsystemBase {
|
||||
using State = frc::TrapezoidProfile::State;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Creates a new ProfiledPIDSubsystem.
|
||||
*
|
||||
* @param controller the ProfiledPIDController to use
|
||||
*/
|
||||
explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
|
||||
|
||||
void Periodic() override;
|
||||
|
||||
/**
|
||||
* Uses the output from the ProfiledPIDController.
|
||||
*
|
||||
* @param output the output of the ProfiledPIDController
|
||||
*/
|
||||
virtual void UseOutput(double output, State state) = 0;
|
||||
|
||||
/**
|
||||
* Returns the goal used by the ProfiledPIDController.
|
||||
*
|
||||
* @return the goal to be used by the controller
|
||||
*/
|
||||
virtual State GetGoal() = 0;
|
||||
|
||||
/**
|
||||
* Returns the measurement of the process variable used by the
|
||||
* ProfiledPIDController.
|
||||
*
|
||||
* @return the measurement of the process variable
|
||||
*/
|
||||
virtual units::meter_t GetMeasurement() = 0;
|
||||
|
||||
/**
|
||||
* Enables the PID control. Resets the controller.
|
||||
*/
|
||||
virtual void Enable();
|
||||
|
||||
/**
|
||||
* Disables the PID control. Sets output to zero.
|
||||
*/
|
||||
virtual void Disable();
|
||||
|
||||
/**
|
||||
* Returns the ProfiledPIDController.
|
||||
*
|
||||
* @return The controller.
|
||||
*/
|
||||
frc::ProfiledPIDController& GetController();
|
||||
|
||||
protected:
|
||||
frc::ProfiledPIDController m_controller;
|
||||
bool m_enabled;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,50 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/SmallVector.h>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
#include "SetUtilities.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* Schedules the given commands when this command is initialized, and ends when
|
||||
* all the commands are no longer scheduled. Useful for forking off from
|
||||
* CommandGroups. If this command is interrupted, it will cancel all of the
|
||||
* commands.
|
||||
*/
|
||||
class ProxyScheduleCommand
|
||||
: public CommandHelper<CommandBase, ProxyScheduleCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new ProxyScheduleCommand that schedules the given commands when
|
||||
* initialized, and ends when they are all no longer scheduled.
|
||||
*
|
||||
* @param toSchedule the commands to schedule
|
||||
*/
|
||||
explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
|
||||
|
||||
ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
|
||||
|
||||
ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
wpi::SmallVector<Command*, 4> m_toSchedule;
|
||||
bool m_finished{false};
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,148 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc/controller/RamseteController.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "frc2/Timer.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that uses a RAMSETE controller to follow a trajectory
|
||||
* with a differential drive.
|
||||
*
|
||||
* <p>The command handles trajectory-following, PID calculations, and
|
||||
* feedforwards internally. This is intended to be a more-or-less "complete
|
||||
* solution" that can be used by teams without a great deal of controls
|
||||
* expertise.
|
||||
*
|
||||
* <p>Advanced teams seeking more flexibility (for example, those who wish to
|
||||
* use the onboard PID functionality of a "smart" motor controller) may use the
|
||||
* secondary constructor that omits the PID and feedforward functionality,
|
||||
* returning only the raw wheel speeds from the RAMSETE controller.
|
||||
*
|
||||
* @see RamseteController
|
||||
* @see Trajectory
|
||||
*/
|
||||
class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
|
||||
using voltsecondspermeter =
|
||||
units::compound_unit<units::volt, units::second,
|
||||
units::inverse<units::meter>>;
|
||||
using voltsecondssquaredpermeter =
|
||||
units::compound_unit<units::volt, units::squared<units::second>,
|
||||
units::inverse<units::meter>>;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Constructs a new RamseteCommand that, when executed, will follow the
|
||||
* provided trajectory. PID control and feedforward are handled internally,
|
||||
* and outputs are scaled -1 to 1 for easy consumption by speed controllers.
|
||||
*
|
||||
* <p>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 ks Constant feedforward term for the robot drive.
|
||||
* @param kv Velocity-proportional feedforward term for the robot
|
||||
* drive.
|
||||
* @param ka Acceleration-proportional feedforward term for the
|
||||
* robot drive.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param leftSpeed A function that supplies the speed of the left side
|
||||
* of the robot drive.
|
||||
* @param rightSpeed A function that supplies the speed of the right side
|
||||
* 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<frc::Pose2d()> pose,
|
||||
frc::RamseteController controller, units::volt_t ks,
|
||||
units::unit_t<voltsecondspermeter> kv,
|
||||
units::unit_t<voltsecondssquaredpermeter> ka,
|
||||
frc::DifferentialDriveKinematics kinematics,
|
||||
std::function<units::meters_per_second_t()> leftSpeed,
|
||||
std::function<units::meters_per_second_t()> rightSpeed,
|
||||
frc2::PIDController leftController,
|
||||
frc2::PIDController rightController,
|
||||
std::function<void(units::volt_t, units::volt_t)> output,
|
||||
std::initializer_list<Subsystem*> 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
|
||||
* outputs (in volts) for the robot drive.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::RamseteController controller,
|
||||
frc::DifferentialDriveKinematics kinematics,
|
||||
std::function<void(units::meters_per_second_t,
|
||||
units::meters_per_second_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
frc::Trajectory m_trajectory;
|
||||
std::function<frc::Pose2d()> m_pose;
|
||||
frc::RamseteController m_controller;
|
||||
const units::volt_t m_ks;
|
||||
const units::unit_t<voltsecondspermeter> m_kv;
|
||||
const units::unit_t<voltsecondssquaredpermeter> m_ka;
|
||||
frc::DifferentialDriveKinematics m_kinematics;
|
||||
std::function<units::meters_per_second_t()> m_leftSpeed;
|
||||
std::function<units::meters_per_second_t()> m_rightSpeed;
|
||||
std::unique_ptr<frc2::PIDController> m_leftController;
|
||||
std::unique_ptr<frc2::PIDController> m_rightController;
|
||||
std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
|
||||
m_outputVel;
|
||||
|
||||
Timer m_timer;
|
||||
units::second_t m_prevTime;
|
||||
frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
|
||||
bool m_usePID;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,41 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that runs a Runnable continuously. Has no end condition as-is;
|
||||
* either subclass it or use Command.WithTimeout() or
|
||||
* Command.WithInterrupt() to give it one. If you only wish
|
||||
* to execute a Runnable once, use InstantCommand.
|
||||
*/
|
||||
class RunCommand : public CommandHelper<CommandBase, RunCommand> {
|
||||
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<void()> toRun,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
RunCommand(RunCommand&& other) = default;
|
||||
|
||||
RunCommand(const RunCommand& other) = default;
|
||||
|
||||
void Execute();
|
||||
|
||||
protected:
|
||||
std::function<void()> m_toRun;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,46 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/SmallVector.h>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
#include "SetUtilities.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* Schedules the given commands when this command is initialized. Useful for
|
||||
* forking off from CommandGroups. Note that if run from a CommandGroup, the
|
||||
* group will not know about the status of the scheduled commands, and will
|
||||
* treat this command as finishing instantly.
|
||||
*/
|
||||
class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new ScheduleCommand that schedules the given commands when
|
||||
* initialized.
|
||||
*
|
||||
* @param toSchedule the commands to schedule
|
||||
*/
|
||||
explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
|
||||
|
||||
ScheduleCommand(ScheduleCommand&& other) = default;
|
||||
|
||||
ScheduleCommand(const ScheduleCommand& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
wpi::SmallVector<Command*, 4> m_toSchedule;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,153 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable : 4521)
|
||||
#endif
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandGroupBase.h"
|
||||
#include "PrintCommand.h"
|
||||
|
||||
namespace frc2 {
|
||||
template <typename Key>
|
||||
/**
|
||||
* Runs one of a selection of commands, either using a selector and a key to
|
||||
* command mapping, or a supplier that returns the command directly at runtime.
|
||||
* Does not actually schedule the selected command - rather, the command is run
|
||||
* through this command; this ensures that the command will behave as expected
|
||||
* if used as part of a CommandGroup. Requires the requirements of all included
|
||||
* commands, again to ensure proper functioning when used in a CommandGroup. If
|
||||
* this is undesired, consider using ScheduleCommand.
|
||||
*
|
||||
* <p>As this command contains multiple component commands within it, it is
|
||||
* technically a command group; the command instances that are passed to it
|
||||
* cannot be added to any other groups, or scheduled individually.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*/
|
||||
class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new selectcommand.
|
||||
*
|
||||
* @param commands the map of commands to choose from
|
||||
* @param selector the selector to determine which command to run
|
||||
*/
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
SelectCommand(std::function<Key()> selector,
|
||||
std::pair<Key, Types>... commands)
|
||||
: m_selector{std::move(selector)} {
|
||||
std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
|
||||
|
||||
((void)foo.emplace_back(commands.first,
|
||||
std::make_unique<std::remove_reference_t<Types>>(
|
||||
std::move(commands.second))),
|
||||
...);
|
||||
|
||||
for (auto&& command : foo) {
|
||||
if (!CommandGroupBase::RequireUngrouped(command.second)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto&& command : foo) {
|
||||
this->AddRequirements(command.second->GetRequirements());
|
||||
m_runsWhenDisabled &= command.second->RunsWhenDisabled();
|
||||
m_commands.emplace(std::move(command.first), std::move(command.second));
|
||||
}
|
||||
}
|
||||
|
||||
SelectCommand(
|
||||
std::function<Key()> selector,
|
||||
std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
|
||||
: m_selector{std::move(selector)} {
|
||||
for (auto&& command : commands) {
|
||||
if (!CommandGroupBase::RequireUngrouped(command.second)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto&& command : commands) {
|
||||
this->AddRequirements(command.second->GetRequirements());
|
||||
m_runsWhenDisabled &= command.second->RunsWhenDisabled();
|
||||
m_commands.emplace(std::move(command.first), std::move(command.second));
|
||||
}
|
||||
}
|
||||
|
||||
// No copy constructors for command groups
|
||||
SelectCommand(const SelectCommand& other) = delete;
|
||||
|
||||
// Prevent template expansion from emulating copy ctor
|
||||
SelectCommand(SelectCommand&) = delete;
|
||||
|
||||
/**
|
||||
* Creates a new selectcommand.
|
||||
*
|
||||
* @param toRun a supplier providing the command to run
|
||||
*/
|
||||
explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
|
||||
|
||||
SelectCommand(SelectCommand&& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override { m_selectedCommand->Execute(); }
|
||||
|
||||
void End(bool interrupted) override {
|
||||
return m_selectedCommand->End(interrupted);
|
||||
}
|
||||
|
||||
bool IsFinished() override { return m_selectedCommand->IsFinished(); }
|
||||
|
||||
bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
|
||||
|
||||
protected:
|
||||
std::unique_ptr<Command> TransferOwnership() && override {
|
||||
return std::make_unique<SelectCommand>(std::move(*this));
|
||||
}
|
||||
|
||||
private:
|
||||
std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
|
||||
std::function<Key()> m_selector;
|
||||
std::function<Command*()> m_toRun;
|
||||
Command* m_selectedCommand;
|
||||
bool m_runsWhenDisabled = true;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
void SelectCommand<T>::Initialize() {
|
||||
if (m_selector) {
|
||||
auto find = m_commands.find(m_selector());
|
||||
if (find == m_commands.end()) {
|
||||
m_selectedCommand = new PrintCommand(
|
||||
"SelectCommand selector value does not correspond to any command!");
|
||||
return;
|
||||
}
|
||||
m_selectedCommand = find->second.get();
|
||||
} else {
|
||||
m_selectedCommand = m_toRun();
|
||||
}
|
||||
m_selectedCommand->Initialize();
|
||||
}
|
||||
|
||||
} // namespace frc2
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
@@ -0,0 +1,104 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable : 4521)
|
||||
#endif
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <wpi/ArrayRef.h>
|
||||
|
||||
#include "CommandGroupBase.h"
|
||||
#include "CommandHelper.h"
|
||||
#include "frc/ErrorBase.h"
|
||||
#include "frc/WPIErrors.h"
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
const size_t invalid_index = std::numeric_limits<size_t>::max();
|
||||
|
||||
/**
|
||||
* A CommandGroups that runs a list of commands in sequence.
|
||||
*
|
||||
* <p>As a rule, CommandGroups require the union of the requirements of their
|
||||
* component commands.
|
||||
*/
|
||||
class SequentialCommandGroup
|
||||
: public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new SequentialCommandGroup. The given commands will be run
|
||||
* sequentially, with the CommandGroup finishing when the last command
|
||||
* finishes.
|
||||
*
|
||||
* @param commands the commands to include in this group.
|
||||
*/
|
||||
explicit SequentialCommandGroup(
|
||||
std::vector<std::unique_ptr<Command>>&& commands);
|
||||
|
||||
/**
|
||||
* Creates a new SequentialCommandGroup. The given commands will be run
|
||||
* sequentially, with the CommandGroup finishing when the last command
|
||||
* finishes.
|
||||
*
|
||||
* @param commands the commands to include in this group.
|
||||
*/
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
explicit SequentialCommandGroup(Types&&... commands) {
|
||||
AddCommands(std::forward<Types>(commands)...);
|
||||
}
|
||||
|
||||
SequentialCommandGroup(SequentialCommandGroup&& other) = default;
|
||||
|
||||
// No copy constructors for command groups
|
||||
SequentialCommandGroup(const SequentialCommandGroup&) = delete;
|
||||
|
||||
// Prevent template expansion from emulating copy ctor
|
||||
SequentialCommandGroup(SequentialCommandGroup&) = delete;
|
||||
|
||||
template <class... Types,
|
||||
typename = std::enable_if_t<std::conjunction_v<
|
||||
std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
|
||||
void AddCommands(Types&&... commands) {
|
||||
std::vector<std::unique_ptr<Command>> foo;
|
||||
((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
|
||||
std::forward<Types>(commands))),
|
||||
...);
|
||||
AddCommands(std::move(foo));
|
||||
}
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
|
||||
|
||||
wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
|
||||
size_t m_currentCommandIndex{invalid_index};
|
||||
bool m_runWhenDisabled{true};
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
#ifdef _WIN32
|
||||
#pragma warning(pop)
|
||||
#endif
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/ArrayRef.h>
|
||||
#include <wpi/SmallVector.h>
|
||||
|
||||
namespace frc2 {
|
||||
template <typename T>
|
||||
void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
|
||||
for (auto addCommand : toAdd) {
|
||||
bool exists = false;
|
||||
for (auto existingCommand : vector) {
|
||||
if (addCommand == existingCommand) {
|
||||
exists = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!exists) {
|
||||
vector.emplace_back(addCommand);
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,46 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that runs a given runnable when it is initalized, and another
|
||||
* runnable when it ends. Useful for running and then stopping a motor, or
|
||||
* extending and then retracting a solenoid. Has no end condition as-is; either
|
||||
* subclass it or use Command.WithTimeout() or Command.WithInterrupt() to give
|
||||
* it one.
|
||||
*/
|
||||
class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
|
||||
public:
|
||||
/**
|
||||
* 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<void()> onInit, std::function<void()> onEnd,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
StartEndCommand(StartEndCommand&& other) = default;
|
||||
|
||||
StartEndCommand(const StartEndCommand& other);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
protected:
|
||||
std::function<void()> m_onInit;
|
||||
std::function<void()> m_onEnd;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,88 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
#include <utility>
|
||||
|
||||
namespace frc2 {
|
||||
class Command;
|
||||
/**
|
||||
* A robot subsystem. Subsystems are the basic unit of robot organization in
|
||||
* the Command-based framework; they encapsulate low-level hardware objects
|
||||
* (motor controllers, sensors, etc) and provide methods through which they can
|
||||
* be used by Commands. Subsystems are used by the CommandScheduler's resource
|
||||
* management system to ensure multiple robot actions are not "fighting" over
|
||||
* the same hardware; Commands that use a subsystem should include that
|
||||
* subsystem in their GetRequirements() method, and resources used within a
|
||||
* subsystem should generally remain encapsulated and not be shared by other
|
||||
* parts of the robot.
|
||||
*
|
||||
* <p>Subsystems must be registered with the scheduler with the
|
||||
* CommandScheduler.RegisterSubsystem() method in order for the
|
||||
* Periodic() method to be called. It is recommended that this method be called
|
||||
* from the constructor of users' Subsystem implementations. The
|
||||
* SubsystemBase class offers a simple base for user implementations
|
||||
* that handles this.
|
||||
*
|
||||
* @see Command
|
||||
* @see CommandScheduler
|
||||
* @see SubsystemBase
|
||||
*/
|
||||
class Subsystem {
|
||||
public:
|
||||
~Subsystem();
|
||||
/**
|
||||
* This method is called periodically by the CommandScheduler. Useful for
|
||||
* updating subsystem-specific state that you don't want to offload to a
|
||||
* Command. Teams should try to be consistent within their own codebases
|
||||
* about which responsibilities will be handled by Commands, and which will be
|
||||
* handled here.
|
||||
*/
|
||||
virtual void Periodic();
|
||||
|
||||
/**
|
||||
* Sets the default Command of the subsystem. The default command will be
|
||||
* automatically scheduled when no other commands are scheduled that require
|
||||
* the subsystem. Default commands should generally not end on their own, i.e.
|
||||
* their IsFinished() method should always return false. Will automatically
|
||||
* register this subsystem with the CommandScheduler.
|
||||
*
|
||||
* @param defaultCommand the default command to associate with this subsystem
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
void SetDefaultCommand(T&& defaultCommand) {
|
||||
CommandScheduler::GetInstance().SetDefaultCommand(
|
||||
this, std::forward<T>(defaultCommand));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the default command for this subsystem. Returns null if no default
|
||||
* command is currently associated with the subsystem.
|
||||
*
|
||||
* @return the default command associated with this subsystem
|
||||
*/
|
||||
Command* GetDefaultCommand() const;
|
||||
|
||||
/**
|
||||
* Returns the command currently running on this subsystem. Returns null if
|
||||
* no command is currently scheduled that requires this subsystem.
|
||||
*
|
||||
* @return the scheduled command currently requiring this subsystem
|
||||
*/
|
||||
Command* GetCurrentCommand() const;
|
||||
|
||||
/**
|
||||
* Registers this subsystem with the CommandScheduler, allowing its
|
||||
* Periodic() method to be called when the scheduler runs.
|
||||
*/
|
||||
void Register();
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,68 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/smartdashboard/Sendable.h>
|
||||
#include <frc/smartdashboard/SendableHelper.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "Subsystem.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A base for subsystems that handles registration in the constructor, and
|
||||
* provides a more intuitive method for setting the default command.
|
||||
*/
|
||||
class SubsystemBase : public Subsystem,
|
||||
public frc::Sendable,
|
||||
public frc::SendableHelper<SubsystemBase> {
|
||||
public:
|
||||
void InitSendable(frc::SendableBuilder& builder) override;
|
||||
|
||||
/**
|
||||
* Gets the name of this Subsystem.
|
||||
*
|
||||
* @return Name
|
||||
*/
|
||||
std::string GetName() const;
|
||||
|
||||
/**
|
||||
* Sets the name of this Subsystem.
|
||||
*
|
||||
* @param name name
|
||||
*/
|
||||
void SetName(const wpi::Twine& name);
|
||||
|
||||
/**
|
||||
* Gets the subsystem name of this Subsystem.
|
||||
*
|
||||
* @return Subsystem name
|
||||
*/
|
||||
std::string GetSubsystem() const;
|
||||
|
||||
/**
|
||||
* Sets the subsystem name of this Subsystem.
|
||||
*
|
||||
* @param subsystem subsystem name
|
||||
*/
|
||||
void SetSubsystem(const wpi::Twine& name);
|
||||
|
||||
/**
|
||||
* Associate a Sendable with this Subsystem.
|
||||
* Also update the child's name.
|
||||
*
|
||||
* @param name name to give child
|
||||
* @param child sendable
|
||||
*/
|
||||
void AddChild(std::string name, frc::Sendable* child);
|
||||
|
||||
protected:
|
||||
SubsystemBase();
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,55 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <frc2/Timer.h>
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that runs a TrapezoidProfile. Useful for smoothly controlling
|
||||
* mechanism motion.
|
||||
*
|
||||
* @see TrapezoidProfile
|
||||
*/
|
||||
class TrapezoidProfileCommand
|
||||
: public CommandHelper<CommandBase, TrapezoidProfileCommand> {
|
||||
public:
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
TrapezoidProfileCommand(
|
||||
frc::TrapezoidProfile profile,
|
||||
std::function<void(frc::TrapezoidProfile::State)> output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
frc::TrapezoidProfile m_profile;
|
||||
std::function<void(frc::TrapezoidProfile::State)> m_output;
|
||||
|
||||
Timer m_timer;
|
||||
};
|
||||
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,51 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/units.h>
|
||||
#include <wpi/Twine.h>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
#include "frc2/Timer.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that does nothing but takes a specified amount of time to finish.
|
||||
* Useful for CommandGroups. Can also be subclassed to make a command with an
|
||||
* internal timer.
|
||||
*/
|
||||
class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new WaitCommand. This command will do nothing, and end after the
|
||||
* specified duration.
|
||||
*
|
||||
* @param duration the time to wait
|
||||
*/
|
||||
explicit WaitCommand(units::second_t duration);
|
||||
|
||||
WaitCommand(WaitCommand&& other) = default;
|
||||
|
||||
WaitCommand(const WaitCommand& other) = default;
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
protected:
|
||||
Timer m_timer;
|
||||
|
||||
private:
|
||||
units::second_t m_duration;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,52 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "frc/Timer.h"
|
||||
#include "frc2/command/CommandHelper.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A command that does nothing but ends after a specified match time or
|
||||
* condition. Useful for CommandGroups.
|
||||
*/
|
||||
class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
|
||||
public:
|
||||
/**
|
||||
* Creates a new WaitUntilCommand that ends after a given condition becomes
|
||||
* true.
|
||||
*
|
||||
* @param condition the condition to determine when to end
|
||||
*/
|
||||
explicit WaitUntilCommand(std::function<bool()> condition);
|
||||
|
||||
/**
|
||||
* Creates a new WaitUntilCommand that ends after a given match time.
|
||||
*
|
||||
* <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this
|
||||
* command does NOT guarantee that the time at which the action is performed
|
||||
* will be judged to be legal by the referees. When in doubt, add a safety
|
||||
* factor or time the action manually.
|
||||
*
|
||||
* @param time the match time after which to end, in seconds
|
||||
*/
|
||||
explicit WaitUntilCommand(double time);
|
||||
|
||||
WaitUntilCommand(WaitUntilCommand&& other) = default;
|
||||
|
||||
WaitUntilCommand(const WaitUntilCommand& other) = default;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
bool RunsWhenDisabled() const override;
|
||||
|
||||
private:
|
||||
std::function<bool()> m_condition;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,207 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
#include <utility>
|
||||
|
||||
#include "Trigger.h"
|
||||
|
||||
namespace frc2 {
|
||||
class Command;
|
||||
/**
|
||||
* A class used to bind command scheduling to button presses. Can be composed
|
||||
* with other buttons with the operators in Trigger.
|
||||
*
|
||||
* @see Trigger
|
||||
*/
|
||||
class Button : public Trigger {
|
||||
public:
|
||||
/**
|
||||
* Create a new button that is pressed when the given condition is true.
|
||||
*
|
||||
* @param isActive Whether the button is pressed.
|
||||
*/
|
||||
explicit Button(std::function<bool()> isPressed);
|
||||
|
||||
/**
|
||||
* Create a new button that is pressed active (default constructor) - activity
|
||||
* can be further determined by subclass code.
|
||||
*/
|
||||
Button() = default;
|
||||
|
||||
/**
|
||||
* Binds a command to start when the button is pressed. Takes a
|
||||
* raw pointer, and so is non-owning; users are responsible for the lifespan
|
||||
* of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Button WhenPressed(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* 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 - rvalue refs will be *moved*, lvalue refs will be
|
||||
* *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Button WhenPressed(T&& command, bool interruptible = true) {
|
||||
WhenActive(std::forward<T>(command), interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a runnable to execute when the button is pressed.
|
||||
*
|
||||
* @param toRun the runnable to execute.
|
||||
*/
|
||||
Button WhenPressed(std::function<void()> toRun);
|
||||
|
||||
/**
|
||||
* Binds a command to be started repeatedly while the button is pressed, and
|
||||
* cancelled when it is released. Takes a raw pointer, and so is non-owning;
|
||||
* users are responsible for the lifespan of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
Button WhileHeld(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to be started repeatedly while the button is pressed, and
|
||||
* cancelled when it is released. Transfers command ownership to the button
|
||||
* scheduler, so the user does not have to worry about lifespan - rvalue refs
|
||||
* will be *moved*, lvalue refs will be *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Button WhileHeld(T&& command, bool interruptible = true) {
|
||||
WhileActiveContinous(std::forward<T>(command), interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a runnable to execute repeatedly while the button is pressed.
|
||||
*
|
||||
* @param toRun the runnable to execute.
|
||||
*/
|
||||
Button WhileHeld(std::function<void()> toRun);
|
||||
|
||||
/**
|
||||
* Binds a command to be started when the button is pressed, and cancelled
|
||||
* when it is released. Takes a raw pointer, and so is non-owning; users are
|
||||
* responsible for the lifespan of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
Button WhenHeld(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to be started when the button is pressed, and cancelled
|
||||
* when it is released. Transfers command ownership to the button scheduler,
|
||||
* so the user does not have to worry about lifespan - rvalue refs will be
|
||||
* *moved*, lvalue refs will be *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Button WhenHeld(T&& command, bool interruptible = true) {
|
||||
WhileActiveOnce(std::forward<T>(command), interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a command to start when the button is released. Takes a
|
||||
* raw pointer, and so is non-owning; users are responsible for the lifespan
|
||||
* of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
Button WhenReleased(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* 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 - rvalue refs will be *moved*, lvalue refs will be
|
||||
* *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Button WhenReleased(T&& command, bool interruptible = true) {
|
||||
WhenInactive(std::forward<T>(command), interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a runnable to execute when the button is released.
|
||||
*
|
||||
* @param toRun the runnable to execute.
|
||||
*/
|
||||
Button WhenReleased(std::function<void()> toRun);
|
||||
|
||||
/**
|
||||
* Binds a command to start when the button is pressed, and be cancelled when
|
||||
* it is pressed again. Takes a raw pointer, and so is non-owning; users are
|
||||
* responsible for the lifespan of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
Button ToggleWhenPressed(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to start when the button is pressed, and be cancelled when
|
||||
* it is pessed again. Transfers command ownership to the button scheduler,
|
||||
* so the user does not have to worry about lifespan - rvalue refs will be
|
||||
* *moved*, lvalue refs will be *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Button ToggleWhenPressed(T&& command, bool interruptible = true) {
|
||||
ToggleWhenActive(std::forward<T>(command), interruptible);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a command to be cancelled when the button is pressed. Takes a
|
||||
* raw pointer, and so is non-owning; users are responsible for the lifespan
|
||||
* and scheduling of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @return The button, for chained calls.
|
||||
*/
|
||||
Button CancelWhenPressed(Command* command);
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,37 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
#include <frc/GenericHID.h>
|
||||
|
||||
#include "Button.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A class used to bind command scheduling to joystick button presses. Can be
|
||||
* composed with other buttons with the operators in Trigger.
|
||||
*
|
||||
* @see Trigger
|
||||
*/
|
||||
class JoystickButton : public Button {
|
||||
public:
|
||||
/**
|
||||
* Creates a JoystickButton that commands can be bound to.
|
||||
*
|
||||
* @param joystick The joystick on which the button is located.
|
||||
* @param buttonNumber The number of the button on the joystic.
|
||||
*/
|
||||
explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
|
||||
: m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
|
||||
|
||||
bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
|
||||
|
||||
private:
|
||||
frc::GenericHID* m_joystick;
|
||||
int m_buttonNumber;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,41 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
#include <frc/GenericHID.h>
|
||||
|
||||
#include "Button.h"
|
||||
|
||||
namespace frc2 {
|
||||
/**
|
||||
* A class used to bind command scheduling to joystick POV presses. Can be
|
||||
* composed with other buttons with the operators in Trigger.
|
||||
*
|
||||
* @see Trigger
|
||||
*/
|
||||
class POVButton : public Button {
|
||||
public:
|
||||
/**
|
||||
* Creates a POVButton that commands can be bound to.
|
||||
*
|
||||
* @param joystick The joystick on which the button is located.
|
||||
* @param angle The angle of the POV corresponding to a button press.
|
||||
* @param povNumber The number of the POV on the joystick.
|
||||
*/
|
||||
POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
|
||||
: m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
|
||||
|
||||
bool Get() const override {
|
||||
return m_joystick->GetPOV(m_povNumber) == m_angle;
|
||||
}
|
||||
|
||||
private:
|
||||
frc::GenericHID* m_joystick;
|
||||
int m_angle;
|
||||
int m_povNumber;
|
||||
};
|
||||
} // namespace frc2
|
||||
@@ -0,0 +1,325 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
namespace frc2 {
|
||||
class Command;
|
||||
/**
|
||||
* A class used to bind command scheduling to events. The
|
||||
* Trigger class is a base for all command-event-binding classes, and so the
|
||||
* methods are named fairly abstractly; for purpose-specific wrappers, see
|
||||
* Button.
|
||||
*
|
||||
* @see Button
|
||||
*/
|
||||
class Trigger {
|
||||
public:
|
||||
/**
|
||||
* Create a new trigger that is active when the given condition is true.
|
||||
*
|
||||
* @param isActive Whether the trigger is active.
|
||||
*/
|
||||
explicit Trigger(std::function<bool()> isActive)
|
||||
: m_isActive{std::move(isActive)} {}
|
||||
|
||||
/**
|
||||
* Create a new trigger that is never active (default constructor) - activity
|
||||
* can be further determined by subclass code.
|
||||
*/
|
||||
Trigger() {
|
||||
m_isActive = [] { return false; };
|
||||
}
|
||||
|
||||
Trigger(const Trigger& other);
|
||||
|
||||
/**
|
||||
* Returns whether the trigger is active. Can be overridden by a subclass.
|
||||
*
|
||||
* @return Whether the trigger is active.
|
||||
*/
|
||||
virtual bool Get() const { return m_isActive(); }
|
||||
|
||||
/**
|
||||
* Binds a command to start when the trigger becomes active. Takes a
|
||||
* raw pointer, and so is non-owning; users are responsible for the lifespan
|
||||
* of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Trigger WhenActive(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to start when the trigger becomes active. Transfers
|
||||
* command ownership to the button scheduler, so the user does not have to
|
||||
* worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
|
||||
* *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Trigger WhenActive(T&& command, bool interruptible = true) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this,
|
||||
command = std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(command)),
|
||||
interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
command->Schedule(interruptible);
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a runnable to execute when the trigger becomes active.
|
||||
*
|
||||
* @param toRun the runnable to execute.
|
||||
*/
|
||||
Trigger WhenActive(std::function<void()> toRun);
|
||||
|
||||
/**
|
||||
* Binds a command to be started repeatedly while the trigger is active, and
|
||||
* cancelled when it becomes inactive. Takes a raw pointer, and so is
|
||||
* non-owning; users are responsible for the lifespan of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Trigger WhileActiveContinous(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to be started repeatedly while the trigger is active, and
|
||||
* cancelled when it becomes inactive. Transfers command ownership to the
|
||||
* button scheduler, so the user does not have to worry about lifespan -
|
||||
* rvalue refs will be *moved*, lvalue refs will be *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this,
|
||||
command = std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(command)),
|
||||
interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (pressed) {
|
||||
command->Schedule(interruptible);
|
||||
} else if (pressedLast && !pressed) {
|
||||
command->Cancel();
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a runnable to execute repeatedly while the trigger is active.
|
||||
*
|
||||
* @param toRun the runnable to execute.
|
||||
*/
|
||||
Trigger WhileActiveContinous(std::function<void()> toRun);
|
||||
|
||||
/**
|
||||
* Binds a command to be started when the trigger becomes active, and
|
||||
* cancelled when it becomes inactive. Takes a raw pointer, and so is
|
||||
* non-owning; users are responsible for the lifespan of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Trigger WhileActiveOnce(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to be started when the trigger becomes active, and
|
||||
* cancelled when it becomes inactive. Transfers command ownership to the
|
||||
* button scheduler, so the user does not have to worry about lifespan -
|
||||
* rvalue refs will be *moved*, lvalue refs will be *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this,
|
||||
command = std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(command)),
|
||||
interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
command->Schedule(interruptible);
|
||||
} else if (pressedLast && !pressed) {
|
||||
command->Cancel();
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a command to start when the trigger becomes inactive. Takes a
|
||||
* raw pointer, and so is non-owning; users are responsible for the lifespan
|
||||
* of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Trigger WhenInactive(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to start when the trigger becomes inactive. Transfers
|
||||
* command ownership to the button scheduler, so the user does not have to
|
||||
* worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
|
||||
* *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Trigger WhenInactive(T&& command, bool interruptible = true) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this,
|
||||
command = std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(command)),
|
||||
interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (pressedLast && !pressed) {
|
||||
command->Schedule(interruptible);
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a runnable to execute when the trigger becomes inactive.
|
||||
*
|
||||
* @param toRun the runnable to execute.
|
||||
*/
|
||||
Trigger WhenInactive(std::function<void()> toRun);
|
||||
|
||||
/**
|
||||
* Binds a command to start when the trigger becomes active, and be cancelled
|
||||
* when it again becomes active. Takes a raw pointer, and so is non-owning;
|
||||
* users are responsible for the lifespan of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Trigger ToggleWhenActive(Command* command, bool interruptible = true);
|
||||
|
||||
/**
|
||||
* Binds a command to start when the trigger becomes active, and be cancelled
|
||||
* when it again becomes active. Transfers command ownership to the button
|
||||
* scheduler, so the user does not have to worry about lifespan - rvalue refs
|
||||
* will be *moved*, lvalue refs will be *copied.*
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @param interruptible Whether the command should be interruptible.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
template <class T, typename = std::enable_if_t<std::is_base_of_v<
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
|
||||
CommandScheduler::GetInstance().AddButton(
|
||||
[pressedLast = Get(), *this,
|
||||
command = std::make_unique<std::remove_reference_t<T>>(
|
||||
std::forward<T>(command)),
|
||||
interruptible]() mutable {
|
||||
bool pressed = Get();
|
||||
|
||||
if (!pressedLast && pressed) {
|
||||
if (command->IsScheduled()) {
|
||||
command->Cancel();
|
||||
} else {
|
||||
command->Schedule(interruptible);
|
||||
}
|
||||
}
|
||||
|
||||
pressedLast = pressed;
|
||||
});
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Binds a command to be cancelled when the trigger becomes active. Takes a
|
||||
* raw pointer, and so is non-owning; users are responsible for the lifespan
|
||||
* and scheduling of the command.
|
||||
*
|
||||
* @param command The command to bind.
|
||||
* @return The trigger, for chained calls.
|
||||
*/
|
||||
Trigger CancelWhenActive(Command* command);
|
||||
|
||||
/**
|
||||
* Composes two triggers with logical AND.
|
||||
*
|
||||
* @return A trigger which is active when both component triggers are active.
|
||||
*/
|
||||
Trigger operator&&(Trigger rhs) {
|
||||
return Trigger([*this, rhs] { return Get() && rhs.Get(); });
|
||||
}
|
||||
|
||||
/**
|
||||
* Composes two triggers with logical OR.
|
||||
*
|
||||
* @return A trigger which is active when either component trigger is active.
|
||||
*/
|
||||
Trigger operator||(Trigger rhs) {
|
||||
return Trigger([*this, rhs] { return Get() || rhs.Get(); });
|
||||
}
|
||||
|
||||
/**
|
||||
* Composes a trigger with logical NOT.
|
||||
*
|
||||
* @return A trigger which is active when the component trigger is inactive,
|
||||
* and vice-versa.
|
||||
*/
|
||||
Trigger operator!() {
|
||||
return Trigger([*this] { return !Get(); });
|
||||
}
|
||||
|
||||
private:
|
||||
std::function<bool()> m_isActive;
|
||||
};
|
||||
} // namespace frc2
|
||||
Reference in New Issue
Block a user