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
@@ -0,0 +1,309 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Set;
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
/**
|
||||
* A state machine representing a complete action to be performed by the robot. Commands are
|
||||
* run by the {@link 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.
|
||||
*/
|
||||
@SuppressWarnings("PMD.TooManyMethods")
|
||||
public interface Command {
|
||||
|
||||
/**
|
||||
* The initial subroutine of a command. Called once when the command is initially scheduled.
|
||||
*/
|
||||
default void initialize() {
|
||||
}
|
||||
|
||||
/**
|
||||
* The main body of a command. Called repeatedly while the command is scheduled.
|
||||
*/
|
||||
default 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
|
||||
*/
|
||||
default void end(boolean 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.
|
||||
*/
|
||||
default boolean 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
|
||||
*/
|
||||
Set<Subsystem> getRequirements();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param seconds the timeout duration
|
||||
* @return the command with the timeout added
|
||||
*/
|
||||
default ParallelRaceGroup withTimeout(double seconds) {
|
||||
return new ParallelRaceGroup(this, new WaitCommand(seconds));
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param condition the interrupt condition
|
||||
* @return the command with the interrupt condition added
|
||||
*/
|
||||
default ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
|
||||
return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command with a runnable to run before this command starts.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param toRun the Runnable to run
|
||||
* @return the decorated command
|
||||
*/
|
||||
default SequentialCommandGroup beforeStarting(Runnable toRun) {
|
||||
return new SequentialCommandGroup(new InstantCommand(toRun), this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command with a runnable to run after the command finishes.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param toRun the Runnable to run
|
||||
* @return the decorated command
|
||||
*/
|
||||
default SequentialCommandGroup andThen(Runnable toRun) {
|
||||
return new SequentialCommandGroup(this, new InstantCommand(toRun));
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command with a set of commands to run after it in sequence. Often more
|
||||
* convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param next the commands to run next
|
||||
* @return the decorated command
|
||||
*/
|
||||
default SequentialCommandGroup andThen(Command... next) {
|
||||
SequentialCommandGroup group = new SequentialCommandGroup(this);
|
||||
group.addCommands(next);
|
||||
return group;
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command with a set of commands to run parallel to it, ending when the calling
|
||||
* command ends and interrupting all the others. Often more convenient/less-verbose than
|
||||
* constructing a new {@link ParallelDeadlineGroup} explicitly.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param parallel the commands to run in parallel
|
||||
* @return the decorated command
|
||||
*/
|
||||
default ParallelDeadlineGroup deadlineWith(Command... parallel) {
|
||||
return new ParallelDeadlineGroup(this, parallel);
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command with a set of commands to run parallel to it, ending when the last
|
||||
* command ends. Often more convenient/less-verbose than constructing a new
|
||||
* {@link ParallelCommandGroup} explicitly.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param parallel the commands to run in parallel
|
||||
* @return the decorated command
|
||||
*/
|
||||
default ParallelCommandGroup alongWith(Command... parallel) {
|
||||
ParallelCommandGroup group = new ParallelCommandGroup(this);
|
||||
group.addCommands(parallel);
|
||||
return group;
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command with a set of commands to run parallel to it, ending when the first
|
||||
* command ends. Often more convenient/less-verbose than constructing a new
|
||||
* {@link ParallelRaceGroup} explicitly.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @param parallel the commands to run in parallel
|
||||
* @return the decorated command
|
||||
*/
|
||||
default ParallelRaceGroup raceWith(Command... parallel) {
|
||||
ParallelRaceGroup group = new ParallelRaceGroup(this);
|
||||
group.addCommands(parallel);
|
||||
return group;
|
||||
}
|
||||
|
||||
/**
|
||||
* Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated
|
||||
* command can still be interrupted or canceled.
|
||||
*
|
||||
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
|
||||
* cannot be used independently after being decorated, or be re-decorated with a different
|
||||
* decorator, unless it is manually cleared from the list of grouped commands with
|
||||
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
|
||||
* further decorated without issue.
|
||||
*
|
||||
* @return the decorated command
|
||||
*/
|
||||
default PerpetualCommand perpetually() {
|
||||
return new PerpetualCommand(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
default ProxyScheduleCommand asProxy() {
|
||||
return new ProxyScheduleCommand(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Schedules this command.
|
||||
*
|
||||
* @param interruptible whether this command can be interrupted by another command that
|
||||
* shares one of its requirements
|
||||
*/
|
||||
default void schedule(boolean interruptible) {
|
||||
CommandScheduler.getInstance().schedule(interruptible, this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Schedules this command, defaulting to interruptible.
|
||||
*/
|
||||
default 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.
|
||||
*/
|
||||
default void cancel() {
|
||||
CommandScheduler.getInstance().cancel(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
default boolean isScheduled() {
|
||||
return CommandScheduler.getInstance().isScheduled(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
default boolean hasRequirement(Subsystem requirement) {
|
||||
return getRequirements().contains(requirement);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
default boolean runsWhenDisabled() {
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the name of this Command.
|
||||
*
|
||||
* @return Name
|
||||
*/
|
||||
default String getName() {
|
||||
return this.getClass().getSimpleName();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.HashSet;
|
||||
import java.util.Set;
|
||||
|
||||
import edu.wpi.first.wpilibj.Sendable;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
|
||||
|
||||
/**
|
||||
* A {@link Sendable} base class for {@link Command}s.
|
||||
*/
|
||||
@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
|
||||
public abstract class CommandBase implements Sendable, Command {
|
||||
|
||||
protected Set<Subsystem> m_requirements = new HashSet<>();
|
||||
|
||||
protected CommandBase() {
|
||||
String name = getClass().getName();
|
||||
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the specified requirements to the command.
|
||||
*
|
||||
* @param requirements the requirements to add
|
||||
*/
|
||||
public final void addRequirements(Subsystem... requirements) {
|
||||
m_requirements.addAll(Set.of(requirements));
|
||||
}
|
||||
|
||||
@Override
|
||||
public Set<Subsystem> getRequirements() {
|
||||
return m_requirements;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return SendableRegistry.getName(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the name of this Command.
|
||||
*
|
||||
* @param name name
|
||||
*/
|
||||
@Override
|
||||
public void setName(String name) {
|
||||
SendableRegistry.setName(this, name);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the subsystem name of this Command.
|
||||
*
|
||||
* @return Subsystem name
|
||||
*/
|
||||
@Override
|
||||
public String getSubsystem() {
|
||||
return SendableRegistry.getSubsystem(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the subsystem name of this Command.
|
||||
*
|
||||
* @param subsystem subsystem name
|
||||
*/
|
||||
@Override
|
||||
public void setSubsystem(String subsystem) {
|
||||
SendableRegistry.setSubsystem(this, subsystem);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
|
||||
*
|
||||
* @param builder the builder used to construct this sendable
|
||||
*/
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Command");
|
||||
builder.addStringProperty(".name", this::getName, null);
|
||||
builder.addBooleanProperty("running", this::isScheduled, value -> {
|
||||
if (value) {
|
||||
if (!isScheduled()) {
|
||||
schedule();
|
||||
}
|
||||
} else {
|
||||
if (isScheduled()) {
|
||||
cancel();
|
||||
}
|
||||
}
|
||||
});
|
||||
builder.addBooleanProperty(".isParented",
|
||||
() -> CommandGroupBase.getGroupedCommands().contains(this), null);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Collection;
|
||||
import java.util.Collections;
|
||||
import java.util.Set;
|
||||
import java.util.WeakHashMap;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public abstract class CommandGroupBase extends CommandBase implements Command {
|
||||
private static final Set<Command> m_groupedCommands =
|
||||
Collections.newSetFromMap(new WeakHashMap<>());
|
||||
|
||||
static void registerGroupedCommands(Command... commands) {
|
||||
m_groupedCommands.addAll(Set.of(commands));
|
||||
}
|
||||
|
||||
/**
|
||||
* Clears the list of grouped commands, allowing all commands to be freely used again.
|
||||
*
|
||||
* <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not
|
||||
* use this unless you fully understand what you are doing.
|
||||
*/
|
||||
public static void clearGroupedCommands() {
|
||||
m_groupedCommands.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* Removes a single command from the list of grouped commands, allowing it to be freely used
|
||||
* again.
|
||||
*
|
||||
* <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not
|
||||
* use this unless you fully understand what you are doing.
|
||||
*
|
||||
* @param command the command to remove from the list of grouped commands
|
||||
*/
|
||||
public static void clearGroupedCommand(Command command) {
|
||||
m_groupedCommands.remove(command);
|
||||
}
|
||||
|
||||
/**
|
||||
* Requires that the specified commands not have been already allocated to a CommandGroup. Throws
|
||||
* an {@link IllegalArgumentException} if commands have been allocated.
|
||||
*
|
||||
* @param commands The commands to check
|
||||
*/
|
||||
public static void requireUngrouped(Command... commands) {
|
||||
requireUngrouped(Set.of(commands));
|
||||
}
|
||||
|
||||
/**
|
||||
* Requires that the specified commands not have been already allocated to a CommandGroup. Throws
|
||||
* an {@link IllegalArgumentException} if commands have been allocated.
|
||||
*
|
||||
* @param commands The commands to check
|
||||
*/
|
||||
public static void requireUngrouped(Collection<Command> commands) {
|
||||
if (!Collections.disjoint(commands, getGroupedCommands())) {
|
||||
throw new IllegalArgumentException("Commands cannot be added to more than one CommandGroup");
|
||||
}
|
||||
}
|
||||
|
||||
static Set<Command> getGroupedCommands() {
|
||||
return m_groupedCommands;
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the given commands to the command group.
|
||||
*
|
||||
* @param commands The commands to add.
|
||||
*/
|
||||
public abstract void addCommands(Command... commands);
|
||||
|
||||
/**
|
||||
* Factory method for {@link SequentialCommandGroup}, included for brevity/convenience.
|
||||
*
|
||||
* @param commands the commands to include
|
||||
* @return the command group
|
||||
*/
|
||||
public static CommandGroupBase sequence(Command... commands) {
|
||||
return new SequentialCommandGroup(commands);
|
||||
}
|
||||
|
||||
/**
|
||||
* Factory method for {@link ParallelCommandGroup}, included for brevity/convenience.
|
||||
*
|
||||
* @param commands the commands to include
|
||||
* @return the command group
|
||||
*/
|
||||
public static CommandGroupBase parallel(Command... commands) {
|
||||
return new ParallelCommandGroup(commands);
|
||||
}
|
||||
|
||||
/**
|
||||
* Factory method for {@link ParallelRaceGroup}, included for brevity/convenience.
|
||||
*
|
||||
* @param commands the commands to include
|
||||
* @return the command group
|
||||
*/
|
||||
public static CommandGroupBase race(Command... commands) {
|
||||
return new ParallelRaceGroup(commands);
|
||||
}
|
||||
|
||||
/**
|
||||
* Factory method for {@link ParallelDeadlineGroup}, included for brevity/convenience.
|
||||
*
|
||||
* @param deadline the deadline command
|
||||
* @param commands the commands to include
|
||||
* @return the command group
|
||||
*/
|
||||
public static CommandGroupBase deadline(Command deadline, Command... commands) {
|
||||
return new ParallelDeadlineGroup(deadline, commands);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,509 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collection;
|
||||
import java.util.Collections;
|
||||
import java.util.Iterator;
|
||||
import java.util.LinkedHashMap;
|
||||
import java.util.LinkedHashSet;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
import java.util.Set;
|
||||
import java.util.function.Consumer;
|
||||
|
||||
import edu.wpi.first.hal.FRCNetComm.tInstances;
|
||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.wpilibj.RobotState;
|
||||
import edu.wpi.first.wpilibj.Sendable;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
|
||||
|
||||
/**
|
||||
* The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link
|
||||
* CommandScheduler#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
|
||||
* {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link
|
||||
* Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
|
||||
*/
|
||||
@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
|
||||
public final class CommandScheduler implements Sendable {
|
||||
/**
|
||||
* The Singleton Instance.
|
||||
*/
|
||||
private static CommandScheduler instance;
|
||||
|
||||
/**
|
||||
* Returns the Scheduler instance.
|
||||
*
|
||||
* @return the instance
|
||||
*/
|
||||
public static synchronized CommandScheduler getInstance() {
|
||||
if (instance == null) {
|
||||
instance = new CommandScheduler();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
|
||||
//A map from commands to their scheduling state. Also used as a set of the currently-running
|
||||
//commands.
|
||||
private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
|
||||
|
||||
//A map from required subsystems to their requiring commands. Also used as a set of the
|
||||
//currently-required subsystems.
|
||||
private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
|
||||
|
||||
//A map from subsystems registered with the scheduler to their default commands. Also used
|
||||
//as a list of currently-registered subsystems.
|
||||
private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>();
|
||||
|
||||
//The set of currently-registered buttons that will be polled every iteration.
|
||||
private final Collection<Runnable> m_buttons = new LinkedHashSet<>();
|
||||
|
||||
private boolean m_disabled;
|
||||
|
||||
//NetworkTable entries for use in Sendable impl
|
||||
private NetworkTableEntry m_namesEntry;
|
||||
private NetworkTableEntry m_idsEntry;
|
||||
private NetworkTableEntry m_cancelEntry;
|
||||
|
||||
//Lists of user-supplied actions to be executed on scheduling events for every command.
|
||||
private final List<Consumer<Command>> m_initActions = new ArrayList<>();
|
||||
private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
|
||||
private final List<Consumer<Command>> m_interruptActions = new ArrayList<>();
|
||||
private final List<Consumer<Command>> m_finishActions = new ArrayList<>();
|
||||
|
||||
// Flag and queues for avoiding ConcurrentModificationException if commands are
|
||||
// scheduled/canceled during run
|
||||
private boolean m_inRunLoop;
|
||||
private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
|
||||
private final List<Command> m_toCancel = new ArrayList<>();
|
||||
|
||||
|
||||
CommandScheduler() {
|
||||
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
|
||||
SendableRegistry.addLW(this, "Scheduler");
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a button binding to the scheduler, which will be polled to schedule commands.
|
||||
*
|
||||
* @param button The button to add
|
||||
*/
|
||||
public void addButton(Runnable button) {
|
||||
m_buttons.add(button);
|
||||
}
|
||||
|
||||
/**
|
||||
* Removes all button bindings from the scheduler.
|
||||
*/
|
||||
public void clearButtons() {
|
||||
m_buttons.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes a given command, adds its requirements to the list, and performs the init actions.
|
||||
*
|
||||
* @param command The command to initialize
|
||||
* @param interruptible Whether the command is interruptible
|
||||
* @param requirements The command requirements
|
||||
*/
|
||||
private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
|
||||
command.initialize();
|
||||
CommandState scheduledCommand = new CommandState(interruptible);
|
||||
m_scheduledCommands.put(command, scheduledCommand);
|
||||
for (Consumer<Command> action : m_initActions) {
|
||||
action.accept(command);
|
||||
}
|
||||
for (Subsystem requirement : requirements) {
|
||||
m_requirements.put(requirement, command);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Schedules a command for execution. Does nothing if the command is already scheduled. If a
|
||||
* command's requirements are not available, it will only be 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
|
||||
*/
|
||||
@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
|
||||
private void schedule(boolean interruptible, Command command) {
|
||||
if (m_inRunLoop) {
|
||||
m_toSchedule.put(command, interruptible);
|
||||
return;
|
||||
}
|
||||
|
||||
if (CommandGroupBase.getGroupedCommands().contains(command)) {
|
||||
throw new IllegalArgumentException(
|
||||
"A command that is part of a command group cannot be independently scheduled");
|
||||
}
|
||||
|
||||
//Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
|
||||
//run when disabled, or the command is already scheduled.
|
||||
if (m_disabled || (RobotState.isDisabled() && !command.runsWhenDisabled())
|
||||
|| m_scheduledCommands.containsKey(command)) {
|
||||
return;
|
||||
}
|
||||
|
||||
Set<Subsystem> requirements = command.getRequirements();
|
||||
|
||||
//Schedule the command if the requirements are not currently in-use.
|
||||
if (Collections.disjoint(m_requirements.keySet(), requirements)) {
|
||||
initCommand(command, interruptible, requirements);
|
||||
} else {
|
||||
//Else check if the requirements that are in use have all have interruptible commands,
|
||||
//and if so, interrupt those commands and schedule the new command.
|
||||
for (Subsystem requirement : requirements) {
|
||||
if (m_requirements.containsKey(requirement)
|
||||
&& !m_scheduledCommands.get(m_requirements.get(requirement)).isInterruptible()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
for (Subsystem requirement : requirements) {
|
||||
if (m_requirements.containsKey(requirement)) {
|
||||
cancel(m_requirements.get(requirement));
|
||||
}
|
||||
}
|
||||
initCommand(command, interruptible, requirements);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public void schedule(boolean interruptible, Command... commands) {
|
||||
for (Command command : commands) {
|
||||
schedule(interruptible, command);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
|
||||
* if the command is already scheduled.
|
||||
*
|
||||
* @param commands the commands to schedule
|
||||
*/
|
||||
public void schedule(Command... commands) {
|
||||
schedule(true, 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.
|
||||
*/
|
||||
@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
|
||||
public void run() {
|
||||
if (m_disabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
//Run the periodic method of all registered subsystems.
|
||||
for (Subsystem subsystem : m_subsystems.keySet()) {
|
||||
subsystem.periodic();
|
||||
}
|
||||
|
||||
//Poll buttons for new commands to add.
|
||||
for (Runnable button : m_buttons) {
|
||||
button.run();
|
||||
}
|
||||
|
||||
m_inRunLoop = true;
|
||||
//Run scheduled commands, remove finished commands.
|
||||
for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
|
||||
iterator.hasNext(); ) {
|
||||
Command command = iterator.next();
|
||||
|
||||
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
|
||||
command.end(true);
|
||||
for (Consumer<Command> action : m_interruptActions) {
|
||||
action.accept(command);
|
||||
}
|
||||
m_requirements.keySet().removeAll(command.getRequirements());
|
||||
iterator.remove();
|
||||
continue;
|
||||
}
|
||||
|
||||
command.execute();
|
||||
for (Consumer<Command> action : m_executeActions) {
|
||||
action.accept(command);
|
||||
}
|
||||
if (command.isFinished()) {
|
||||
command.end(false);
|
||||
for (Consumer<Command> action : m_finishActions) {
|
||||
action.accept(command);
|
||||
}
|
||||
iterator.remove();
|
||||
|
||||
m_requirements.keySet().removeAll(command.getRequirements());
|
||||
}
|
||||
}
|
||||
m_inRunLoop = false;
|
||||
|
||||
//Schedule/cancel commands from queues populated during loop
|
||||
for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
|
||||
schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
|
||||
}
|
||||
|
||||
for (Command command : m_toCancel) {
|
||||
cancel(command);
|
||||
}
|
||||
|
||||
m_toSchedule.clear();
|
||||
m_toCancel.clear();
|
||||
|
||||
//Add default commands for un-required registered subsystems.
|
||||
for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
|
||||
if (!m_requirements.containsKey(subsystemCommand.getKey())
|
||||
&& subsystemCommand.getValue() != null) {
|
||||
schedule(subsystemCommand.getValue());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 subsystems the subsystem to register
|
||||
*/
|
||||
public void registerSubsystem(Subsystem... subsystems) {
|
||||
for (Subsystem subsystem : subsystems) {
|
||||
m_subsystems.put(subsystem, null);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 subsystems the subsystem to un-register
|
||||
*/
|
||||
public void unregisterSubsystem(Subsystem... subsystems) {
|
||||
m_subsystems.keySet().removeAll(Set.of(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
|
||||
* {@link Command#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
|
||||
*/
|
||||
public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) {
|
||||
if (!defaultCommand.getRequirements().contains(subsystem)) {
|
||||
throw new IllegalArgumentException("Default commands must require their subsystem!");
|
||||
}
|
||||
|
||||
if (defaultCommand.isFinished()) {
|
||||
throw new IllegalArgumentException("Default commands should not end!");
|
||||
}
|
||||
|
||||
m_subsystems.put(subsystem, 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
|
||||
*/
|
||||
public Command getDefaultCommand(Subsystem subsystem) {
|
||||
return m_subsystems.get(subsystem);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public void cancel(Command... commands) {
|
||||
if (m_inRunLoop) {
|
||||
m_toCancel.addAll(List.of(commands));
|
||||
return;
|
||||
}
|
||||
|
||||
for (Command command : commands) {
|
||||
if (!m_scheduledCommands.containsKey(command)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
command.end(true);
|
||||
for (Consumer<Command> action : m_interruptActions) {
|
||||
action.accept(command);
|
||||
}
|
||||
m_scheduledCommands.remove(command);
|
||||
m_requirements.keySet().removeAll(command.getRequirements());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Cancels all commands that are currently scheduled.
|
||||
*/
|
||||
public void cancelAll() {
|
||||
for (Command command : m_scheduledCommands.keySet()) {
|
||||
cancel(command);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public double timeSinceScheduled(Command command) {
|
||||
CommandState commandState = m_scheduledCommands.get(command);
|
||||
if (commandState != null) {
|
||||
return commandState.timeSinceInitialized();
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public boolean isScheduled(Command... commands) {
|
||||
return m_scheduledCommands.keySet().containsAll(Set.of(commands));
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public Command requiring(Subsystem subsystem) {
|
||||
return m_requirements.get(subsystem);
|
||||
}
|
||||
|
||||
/**
|
||||
* Disables the command scheduler.
|
||||
*/
|
||||
public void disable() {
|
||||
m_disabled = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables the command scheduler.
|
||||
*/
|
||||
public void enable() {
|
||||
m_disabled = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the initialization of any command by the scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
public void onCommandInitialize(Consumer<Command> action) {
|
||||
m_initActions.add(action);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the execution of any command by the scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
public void onCommandExecute(Consumer<Command> action) {
|
||||
m_executeActions.add(action);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the interruption of any command by the scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
public void onCommandInterrupt(Consumer<Command> action) {
|
||||
m_interruptActions.add(action);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds an action to perform on the finishing of any command by the scheduler.
|
||||
*
|
||||
* @param action the action to perform
|
||||
*/
|
||||
public void onCommandFinish(Consumer<Command> action) {
|
||||
m_finishActions.add(action);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Scheduler");
|
||||
m_namesEntry = builder.getEntry("Names");
|
||||
m_idsEntry = builder.getEntry("Ids");
|
||||
m_cancelEntry = builder.getEntry("Cancel");
|
||||
builder.setUpdateTable(() -> {
|
||||
|
||||
if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
Map<Double, Command> ids = new LinkedHashMap<>();
|
||||
|
||||
|
||||
for (Command command : m_scheduledCommands.keySet()) {
|
||||
ids.put((double) command.hashCode(), command);
|
||||
}
|
||||
|
||||
double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
|
||||
if (toCancel.length > 0) {
|
||||
for (double hash : toCancel) {
|
||||
cancel(ids.get(hash));
|
||||
ids.remove(hash);
|
||||
}
|
||||
m_cancelEntry.setDoubleArray(new double[0]);
|
||||
}
|
||||
|
||||
List<String> names = new ArrayList<>();
|
||||
|
||||
ids.values().forEach(command -> names.add(command.getName()));
|
||||
|
||||
m_namesEntry.setStringArray(names.toArray(new String[0]));
|
||||
m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
|
||||
/**
|
||||
* Class that holds scheduling state for a command. Used internally by the
|
||||
* {@link CommandScheduler}.
|
||||
*/
|
||||
class CommandState {
|
||||
//The time since this command was initialized.
|
||||
private double m_startTime = -1;
|
||||
|
||||
//Whether or not it is interruptible.
|
||||
private final boolean m_interruptible;
|
||||
|
||||
CommandState(boolean interruptible) {
|
||||
m_interruptible = interruptible;
|
||||
startTiming();
|
||||
startRunning();
|
||||
}
|
||||
|
||||
private void startTiming() {
|
||||
m_startTime = Timer.getFPGATimestamp();
|
||||
}
|
||||
|
||||
synchronized void startRunning() {
|
||||
m_startTime = -1;
|
||||
}
|
||||
|
||||
boolean isInterruptible() {
|
||||
return m_interruptible;
|
||||
}
|
||||
|
||||
double timeSinceInitialized() {
|
||||
return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
|
||||
|
||||
/**
|
||||
* 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 {@link 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.
|
||||
*/
|
||||
public class ConditionalCommand extends CommandBase {
|
||||
private final Command m_onTrue;
|
||||
private final Command m_onFalse;
|
||||
private final BooleanSupplier m_condition;
|
||||
private Command m_selectedCommand;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) {
|
||||
requireUngrouped(onTrue, onFalse);
|
||||
|
||||
CommandGroupBase.registerGroupedCommands(onTrue, onFalse);
|
||||
|
||||
m_onTrue = onTrue;
|
||||
m_onFalse = onFalse;
|
||||
m_condition = requireNonNullParam(condition, "condition", "ConditionalCommand");
|
||||
m_requirements.addAll(m_onTrue.getRequirements());
|
||||
m_requirements.addAll(m_onFalse.getRequirements());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
if (m_condition.getAsBoolean()) {
|
||||
m_selectedCommand = m_onTrue;
|
||||
} else {
|
||||
m_selectedCommand = m_onFalse;
|
||||
}
|
||||
m_selectedCommand.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_selectedCommand.execute();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_selectedCommand.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_selectedCommand.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_onTrue.runsWhenDisabled() && m_onFalse.runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.Consumer;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class FunctionalCommand extends CommandBase {
|
||||
protected final Runnable m_onInit;
|
||||
protected final Runnable m_onExecute;
|
||||
protected final Consumer<Boolean> m_onEnd;
|
||||
protected final BooleanSupplier m_isFinished;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public FunctionalCommand(Runnable onInit, Runnable onExecute, Consumer<Boolean> onEnd,
|
||||
BooleanSupplier isFinished, Subsystem... requirements) {
|
||||
m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand");
|
||||
m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand");
|
||||
m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand");
|
||||
m_isFinished = requireNonNullParam(isFinished, "isFinished", "FunctionalCommand");
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_onInit.run();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_onExecute.run();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_onEnd.accept(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_isFinished.getAsBoolean();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class InstantCommand extends CommandBase {
|
||||
private final Runnable m_toRun;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public InstantCommand(Runnable toRun, Subsystem... requirements) {
|
||||
m_toRun = requireNonNullParam(toRun, "toRun", "InstantCommand");
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg
|
||||
* constructor to call implicitly from subclass constructors.
|
||||
*/
|
||||
public InstantCommand() {
|
||||
m_toRun = () -> {
|
||||
};
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_toRun.run();
|
||||
}
|
||||
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.Notifier;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class NotifierCommand extends CommandBase {
|
||||
protected final Notifier m_notifier;
|
||||
protected final double m_period;
|
||||
|
||||
/**
|
||||
* Creates a new NotifierCommand.
|
||||
*
|
||||
* @param toRun the runnable for the notifier to run
|
||||
* @param period the period at which the notifier should run, in seconds
|
||||
* @param requirements the subsystems required by this command
|
||||
*/
|
||||
public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
|
||||
m_notifier = new Notifier(toRun);
|
||||
m_period = period;
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_notifier.startPeriodic(m_period);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_notifier.stop();
|
||||
}
|
||||
}
|
||||
@@ -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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Set;
|
||||
import java.util.function.DoubleConsumer;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A command that controls an output with a {@link 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.
|
||||
*/
|
||||
public class PIDCommand extends CommandBase {
|
||||
protected final PIDController m_controller;
|
||||
protected DoubleSupplier m_measurement;
|
||||
protected DoubleSupplier m_setpoint;
|
||||
protected DoubleConsumer m_useOutput;
|
||||
|
||||
/**
|
||||
* 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 setpoint
|
||||
* @param useOutput the controller's output
|
||||
* @param requirements the subsystems required by this command
|
||||
*/
|
||||
public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
|
||||
DoubleSupplier setpointSource, DoubleConsumer useOutput,
|
||||
Subsystem... requirements) {
|
||||
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
|
||||
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
|
||||
requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
|
||||
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
|
||||
|
||||
m_controller = controller;
|
||||
m_useOutput = useOutput;
|
||||
m_measurement = measurementSource;
|
||||
m_setpoint = setpointSource;
|
||||
m_requirements.addAll(Set.of(requirements));
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new PIDCommand, which controls the given output with a PIDController.
|
||||
*
|
||||
* @param controller the controller that controls the output.
|
||||
* @param measurementSource the measurement of the process variable
|
||||
* @param setpoint the controller's setpoint
|
||||
* @param useOutput the controller's output
|
||||
* @param requirements the subsystems required by this command
|
||||
*/
|
||||
public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
|
||||
double setpoint, DoubleConsumer useOutput,
|
||||
Subsystem... requirements) {
|
||||
this(controller, measurementSource, () -> setpoint, useOutput, requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_controller.reset();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
|
||||
m_setpoint.getAsDouble()));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_useOutput.accept(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the PIDController used by the command.
|
||||
*
|
||||
* @return The PIDController
|
||||
*/
|
||||
public PIDController getController() {
|
||||
return m_controller;
|
||||
}
|
||||
}
|
||||
@@ -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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A subsystem that uses a {@link PIDController} to control an output. The controller is run
|
||||
* synchronously from the subsystem's periodic() method.
|
||||
*/
|
||||
public abstract class PIDSubsystem extends SubsystemBase {
|
||||
protected final PIDController m_controller;
|
||||
protected boolean m_enabled;
|
||||
|
||||
/**
|
||||
* Creates a new PIDSubsystem.
|
||||
*
|
||||
* @param controller the PIDController to use
|
||||
*/
|
||||
public PIDSubsystem(PIDController controller) {
|
||||
requireNonNullParam(controller, "controller", "PIDSubsystem");
|
||||
m_controller = controller;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if (m_enabled) {
|
||||
useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
|
||||
}
|
||||
}
|
||||
|
||||
public PIDController getController() {
|
||||
return m_controller;
|
||||
}
|
||||
|
||||
/**
|
||||
* Uses the output from the PIDController.
|
||||
*
|
||||
* @param output the output of the PIDController
|
||||
*/
|
||||
public abstract void useOutput(double output);
|
||||
|
||||
/**
|
||||
* Returns the reference (setpoint) used by the PIDController.
|
||||
*
|
||||
* @return the reference (setpoint) to be used by the controller
|
||||
*/
|
||||
public abstract double getSetpoint();
|
||||
|
||||
/**
|
||||
* Returns the measurement of the process variable used by the PIDController.
|
||||
*
|
||||
* @return the measurement of the process variable
|
||||
*/
|
||||
public abstract double getMeasurement();
|
||||
|
||||
/**
|
||||
* Enables the PID control. Resets the controller.
|
||||
*/
|
||||
public void enable() {
|
||||
m_enabled = true;
|
||||
m_controller.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Disables the PID control. Sets output to zero.
|
||||
*/
|
||||
public void disable() {
|
||||
m_enabled = false;
|
||||
useOutput(0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Collections;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class ParallelCommandGroup extends CommandGroupBase {
|
||||
//maps commands in this group to whether they are still running
|
||||
private final Map<Command, Boolean> m_commands = new HashMap<>();
|
||||
private boolean m_runWhenDisabled = true;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public ParallelCommandGroup(Command... commands) {
|
||||
addCommands(commands);
|
||||
}
|
||||
|
||||
@Override
|
||||
public final void addCommands(Command... commands) {
|
||||
requireUngrouped(commands);
|
||||
|
||||
if (m_commands.containsValue(true)) {
|
||||
throw new IllegalStateException(
|
||||
"Commands cannot be added to a CommandGroup while the group is running");
|
||||
}
|
||||
|
||||
registerGroupedCommands(commands);
|
||||
|
||||
for (Command command : commands) {
|
||||
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
|
||||
throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
|
||||
+ "require the same subsystems");
|
||||
}
|
||||
m_commands.put(command, false);
|
||||
m_requirements.addAll(command.getRequirements());
|
||||
m_runWhenDisabled &= command.runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
|
||||
commandRunning.getKey().initialize();
|
||||
commandRunning.setValue(true);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
|
||||
if (!commandRunning.getValue()) {
|
||||
continue;
|
||||
}
|
||||
commandRunning.getKey().execute();
|
||||
if (commandRunning.getKey().isFinished()) {
|
||||
commandRunning.getKey().end(false);
|
||||
commandRunning.setValue(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
if (interrupted) {
|
||||
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
|
||||
if (commandRunning.getValue()) {
|
||||
commandRunning.getKey().end(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return !m_commands.values().contains(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Collections;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class ParallelDeadlineGroup extends CommandGroupBase {
|
||||
//maps commands in this group to whether they are still running
|
||||
private final Map<Command, Boolean> m_commands = new HashMap<>();
|
||||
private boolean m_runWhenDisabled = true;
|
||||
private Command m_deadline;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public ParallelDeadlineGroup(Command deadline, Command... commands) {
|
||||
m_deadline = deadline;
|
||||
addCommands(commands);
|
||||
if (!m_commands.containsKey(deadline)) {
|
||||
addCommands(deadline);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the deadline to the given command. The deadline is added to the group if it is not
|
||||
* already contained.
|
||||
*
|
||||
* @param deadline the command that determines when the group ends
|
||||
*/
|
||||
public void setDeadline(Command deadline) {
|
||||
if (!m_commands.containsKey(deadline)) {
|
||||
addCommands(deadline);
|
||||
}
|
||||
m_deadline = deadline;
|
||||
}
|
||||
|
||||
@Override
|
||||
public final void addCommands(Command... commands) {
|
||||
requireUngrouped(commands);
|
||||
|
||||
if (m_commands.containsValue(true)) {
|
||||
throw new IllegalStateException(
|
||||
"Commands cannot be added to a CommandGroup while the group is running");
|
||||
}
|
||||
|
||||
registerGroupedCommands(commands);
|
||||
|
||||
for (Command command : commands) {
|
||||
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
|
||||
throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
|
||||
+ "require the same subsystems");
|
||||
}
|
||||
m_commands.put(command, false);
|
||||
m_requirements.addAll(command.getRequirements());
|
||||
m_runWhenDisabled &= command.runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
|
||||
commandRunning.getKey().initialize();
|
||||
commandRunning.setValue(true);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
|
||||
if (!commandRunning.getValue()) {
|
||||
continue;
|
||||
}
|
||||
commandRunning.getKey().execute();
|
||||
if (commandRunning.getKey().isFinished()) {
|
||||
commandRunning.getKey().end(false);
|
||||
commandRunning.setValue(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
|
||||
if (commandRunning.getValue()) {
|
||||
commandRunning.getKey().end(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_deadline.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Collections;
|
||||
import java.util.HashSet;
|
||||
import java.util.Set;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class ParallelRaceGroup extends CommandGroupBase {
|
||||
private final Set<Command> m_commands = new HashSet<>();
|
||||
private boolean m_runWhenDisabled = true;
|
||||
private boolean m_finished = true;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public ParallelRaceGroup(Command... commands) {
|
||||
addCommands(commands);
|
||||
}
|
||||
|
||||
@Override
|
||||
public final void addCommands(Command... commands) {
|
||||
requireUngrouped(commands);
|
||||
|
||||
if (!m_finished) {
|
||||
throw new IllegalStateException(
|
||||
"Commands cannot be added to a CommandGroup while the group is running");
|
||||
}
|
||||
|
||||
registerGroupedCommands(commands);
|
||||
|
||||
for (Command command : commands) {
|
||||
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
|
||||
throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
|
||||
+ " require the same subsystems");
|
||||
}
|
||||
m_commands.add(command);
|
||||
m_requirements.addAll(command.getRequirements());
|
||||
m_runWhenDisabled &= command.runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_finished = false;
|
||||
for (Command command : m_commands) {
|
||||
command.initialize();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
for (Command command : m_commands) {
|
||||
command.execute();
|
||||
if (command.isFinished()) {
|
||||
m_finished = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
for (Command command : m_commands) {
|
||||
command.end(!command.isFinished());
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands;
|
||||
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class PerpetualCommand extends CommandBase {
|
||||
protected final Command m_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
|
||||
*/
|
||||
public PerpetualCommand(Command command) {
|
||||
requireUngrouped(command);
|
||||
registerGroupedCommands(command);
|
||||
m_command = command;
|
||||
m_requirements.addAll(command.getRequirements());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_command.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_command.execute();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_command.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_command.runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
/**
|
||||
* A command that prints a string when initialized.
|
||||
*/
|
||||
public class PrintCommand extends InstantCommand {
|
||||
/**
|
||||
* Creates a new a PrintCommand.
|
||||
*
|
||||
* @param message the message to print
|
||||
*/
|
||||
public PrintCommand(String message) {
|
||||
super(() -> System.out.println(message));
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,138 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Set;
|
||||
import java.util.function.BiConsumer;
|
||||
import java.util.function.DoubleSupplier;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
|
||||
|
||||
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A command that controls an output with a {@link 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.
|
||||
*/
|
||||
public class ProfiledPIDCommand extends CommandBase {
|
||||
protected final ProfiledPIDController m_controller;
|
||||
protected DoubleSupplier m_measurement;
|
||||
protected Supplier<State> m_goal;
|
||||
protected BiConsumer<Double, State> m_useOutput;
|
||||
|
||||
/**
|
||||
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
|
||||
* Goal velocity is specified.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
|
||||
Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
|
||||
Subsystem... requirements) {
|
||||
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
|
||||
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
|
||||
requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
|
||||
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
|
||||
|
||||
m_controller = controller;
|
||||
m_useOutput = useOutput;
|
||||
m_measurement = measurementSource;
|
||||
m_goal = goalSource;
|
||||
m_requirements.addAll(Set.of(requirements));
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
|
||||
* Goal velocity is implicitly zero.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
|
||||
DoubleSupplier goalSource, BiConsumer<Double, State> useOutput,
|
||||
Subsystem... requirements) {
|
||||
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
|
||||
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
|
||||
requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
|
||||
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
|
||||
|
||||
m_controller = controller;
|
||||
m_useOutput = useOutput;
|
||||
m_measurement = measurementSource;
|
||||
m_goal = () -> new State(goalSource.getAsDouble(), 0);
|
||||
m_requirements.addAll(Set.of(requirements));
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
|
||||
* velocity is specified.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
|
||||
State goal, BiConsumer<Double, State> useOutput,
|
||||
Subsystem... requirements) {
|
||||
this(controller, measurementSource, () -> goal, useOutput, requirements);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
|
||||
* velocity is implicitly zero.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
|
||||
double goal, BiConsumer<Double, State> useOutput,
|
||||
Subsystem... requirements) {
|
||||
this(controller, measurementSource, () -> goal, useOutput, requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_controller.reset();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
|
||||
m_controller.getSetpoint());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_useOutput.accept(0., new State());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the ProfiledPIDController used by the command.
|
||||
*
|
||||
* @return The ProfiledPIDController
|
||||
*/
|
||||
public ProfiledPIDController getController() {
|
||||
return m_controller;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
|
||||
|
||||
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is
|
||||
* run synchronously from the subsystem's periodic() method.
|
||||
*/
|
||||
public abstract class ProfiledPIDSubsystem extends SubsystemBase {
|
||||
protected final ProfiledPIDController m_controller;
|
||||
protected boolean m_enabled;
|
||||
|
||||
/**
|
||||
* Creates a new ProfiledPIDSubsystem.
|
||||
*
|
||||
* @param controller the ProfiledPIDController to use
|
||||
*/
|
||||
public ProfiledPIDSubsystem(ProfiledPIDController controller) {
|
||||
requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
|
||||
m_controller = controller;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if (m_enabled) {
|
||||
useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
|
||||
}
|
||||
}
|
||||
|
||||
public ProfiledPIDController getController() {
|
||||
return m_controller;
|
||||
}
|
||||
|
||||
/**
|
||||
* Uses the output from the ProfiledPIDController.
|
||||
*
|
||||
* @param output the output of the ProfiledPIDController
|
||||
* @param goal the goal state of the ProfiledPIDController, for feedforward
|
||||
*/
|
||||
public abstract void useOutput(double output, State goal);
|
||||
|
||||
/**
|
||||
* Returns the goal used by the ProfiledPIDController.
|
||||
*
|
||||
* @return the goal to be used by the controller
|
||||
*/
|
||||
public abstract State getGoal();
|
||||
|
||||
/**
|
||||
* Returns the measurement of the process variable used by the ProfiledPIDController.
|
||||
*
|
||||
* @return the measurement of the process variable
|
||||
*/
|
||||
public abstract double getMeasurement();
|
||||
|
||||
/**
|
||||
* Enables the PID control. Resets the controller.
|
||||
*/
|
||||
public void enable() {
|
||||
m_enabled = true;
|
||||
m_controller.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Disables the PID control. Sets output to zero.
|
||||
*/
|
||||
public void disable() {
|
||||
m_enabled = false;
|
||||
useOutput(0, new State());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Set;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class ProxyScheduleCommand extends CommandBase {
|
||||
private final Set<Command> m_toSchedule;
|
||||
private boolean m_finished;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public ProxyScheduleCommand(Command... toSchedule) {
|
||||
m_toSchedule = Set.of(toSchedule);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
for (Command command : m_toSchedule) {
|
||||
command.schedule();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
if (interrupted) {
|
||||
for (Command command : m_toSchedule) {
|
||||
command.cancel();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_finished = true;
|
||||
for (Command command : m_toSchedule) {
|
||||
m_finished &= !command.isScheduled();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,231 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.BiConsumer;
|
||||
import java.util.function.DoubleSupplier;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.RamseteController;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.trajectory.Trajectory;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
|
||||
* {@link 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.
|
||||
*/
|
||||
@SuppressWarnings("PMD.TooManyFields")
|
||||
public class RamseteCommand extends CommandBase {
|
||||
private final Timer m_timer = new Timer();
|
||||
private DifferentialDriveWheelSpeeds m_prevSpeeds;
|
||||
private double m_prevTime;
|
||||
private final boolean m_usePID;
|
||||
|
||||
private final Trajectory m_trajectory;
|
||||
private final Supplier<Pose2d> m_pose;
|
||||
private final RamseteController m_follower;
|
||||
private final double m_ks;
|
||||
private final double m_kv;
|
||||
private final double m_ka;
|
||||
private final DifferentialDriveKinematics m_kinematics;
|
||||
private final DoubleSupplier m_leftSpeed;
|
||||
private final DoubleSupplier m_rightSpeed;
|
||||
private final PIDController m_leftController;
|
||||
private final PIDController m_rightController;
|
||||
private final BiConsumer<Double, Double> m_output;
|
||||
|
||||
/**
|
||||
* 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 ksVolts Constant feedforward term for the robot drive.
|
||||
* @param kvVoltSecondsPerMeter Velocity-proportional feedforward term for the robot
|
||||
* drive.
|
||||
* @param kaVoltSecondsSquaredPerMeter Acceleration-proportional feedforward term for the robot
|
||||
* drive.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param leftWheelSpeedMetersPerSecond A function that supplies the speed of the left side of
|
||||
* the robot drive.
|
||||
* @param rightWheelSpeedMetersPerSecond 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 outputVolts A function that consumes the computed left and right
|
||||
* outputs (in volts) for the robot drive.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
@SuppressWarnings("PMD.ExcessiveParameterList")
|
||||
public RamseteCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
RamseteController controller,
|
||||
double ksVolts,
|
||||
double kvVoltSecondsPerMeter,
|
||||
double kaVoltSecondsSquaredPerMeter,
|
||||
DifferentialDriveKinematics kinematics,
|
||||
DoubleSupplier leftWheelSpeedMetersPerSecond,
|
||||
DoubleSupplier rightWheelSpeedMetersPerSecond,
|
||||
PIDController leftController,
|
||||
PIDController rightController,
|
||||
BiConsumer<Double, Double> outputVolts,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
|
||||
m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
|
||||
m_ks = ksVolts;
|
||||
m_kv = kvVoltSecondsPerMeter;
|
||||
m_ka = kaVoltSecondsSquaredPerMeter;
|
||||
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
|
||||
m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
|
||||
"leftWheelSpeedMetersPerSecond",
|
||||
"RamseteCommand");
|
||||
m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond,
|
||||
"rightWheelSpeedMetersPerSecond",
|
||||
"RamseteCommand");
|
||||
m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
|
||||
m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
|
||||
m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
|
||||
|
||||
m_usePID = true;
|
||||
|
||||
addRequirements(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 follower The RAMSETE follower used to follow the trajectory.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param outputMetersPerSecond A function that consumes the computed left and right
|
||||
* wheel speeds.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
public RamseteCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
RamseteController follower,
|
||||
DifferentialDriveKinematics kinematics,
|
||||
BiConsumer<Double, Double> outputMetersPerSecond,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
|
||||
m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
|
||||
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
|
||||
m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
|
||||
|
||||
m_ks = 0;
|
||||
m_kv = 0;
|
||||
m_ka = 0;
|
||||
m_leftSpeed = null;
|
||||
m_rightSpeed = null;
|
||||
m_leftController = null;
|
||||
m_rightController = null;
|
||||
|
||||
m_usePID = false;
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_prevTime = 0;
|
||||
var initialState = m_trajectory.sample(0);
|
||||
m_prevSpeeds = m_kinematics.toWheelSpeeds(
|
||||
new ChassisSpeeds(initialState.velocityMetersPerSecond,
|
||||
0,
|
||||
initialState.curvatureRadPerMeter
|
||||
* initialState.velocityMetersPerSecond));
|
||||
m_timer.reset();
|
||||
m_timer.start();
|
||||
if (m_usePID) {
|
||||
m_leftController.reset();
|
||||
m_rightController.reset();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
double curTime = m_timer.get();
|
||||
double dt = curTime - m_prevTime;
|
||||
|
||||
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
|
||||
m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
|
||||
|
||||
var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
|
||||
var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
|
||||
|
||||
double leftOutput;
|
||||
double rightOutput;
|
||||
|
||||
if (m_usePID) {
|
||||
double leftFeedforward =
|
||||
m_ks * Math.signum(leftSpeedSetpoint)
|
||||
+ m_kv * leftSpeedSetpoint
|
||||
+ m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt;
|
||||
|
||||
double rightFeedforward =
|
||||
m_ks * Math.signum(rightSpeedSetpoint)
|
||||
+ m_kv * rightSpeedSetpoint
|
||||
+ m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt;
|
||||
|
||||
leftOutput = leftFeedforward
|
||||
+ m_leftController.calculate(m_leftSpeed.getAsDouble(),
|
||||
leftSpeedSetpoint);
|
||||
|
||||
rightOutput = rightFeedforward
|
||||
+ m_rightController.calculate(m_rightSpeed.getAsDouble(),
|
||||
rightSpeedSetpoint);
|
||||
} else {
|
||||
leftOutput = leftSpeedSetpoint;
|
||||
rightOutput = rightSpeedSetpoint;
|
||||
}
|
||||
|
||||
m_output.accept(leftOutput, rightOutput);
|
||||
|
||||
m_prevTime = curTime;
|
||||
m_prevSpeeds = targetWheelSpeeds;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_timer.stop();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A command that runs a Runnable continuously. Has no end condition as-is;
|
||||
* either subclass it or use {@link Command#withTimeout(double)} or
|
||||
* {@link Command#withInterrupt(BooleanSupplier)} to give it one. If you only wish
|
||||
* to execute a Runnable once, use {@link InstantCommand}.
|
||||
*/
|
||||
public class RunCommand extends CommandBase {
|
||||
protected final Runnable m_toRun;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public RunCommand(Runnable toRun, Subsystem... requirements) {
|
||||
m_toRun = requireNonNullParam(toRun, "toRun", "RunCommand");
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_toRun.run();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Set;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class ScheduleCommand extends CommandBase {
|
||||
private final Set<Command> m_toSchedule;
|
||||
|
||||
/**
|
||||
* Creates a new ScheduleCommand that schedules the given commands when initialized.
|
||||
*
|
||||
* @param toSchedule the commands to schedule
|
||||
*/
|
||||
public ScheduleCommand(Command... toSchedule) {
|
||||
m_toSchedule = Set.of(toSchedule);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
for (Command command : m_toSchedule) {
|
||||
command.schedule();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.Map;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
|
||||
|
||||
/**
|
||||
* 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 {@link 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.
|
||||
*/
|
||||
public class SelectCommand extends CommandBase {
|
||||
private final Map<Object, Command> m_commands;
|
||||
private final Supplier<Object> m_selector;
|
||||
private final Supplier<Command> m_toRun;
|
||||
private Command m_selectedCommand;
|
||||
|
||||
/**
|
||||
* Creates a new selectcommand.
|
||||
*
|
||||
* @param commands the map of commands to choose from
|
||||
* @param selector the selector to determine which command to run
|
||||
*/
|
||||
public SelectCommand(Map<Object, Command> commands, Supplier<Object> selector) {
|
||||
requireUngrouped(commands.values());
|
||||
|
||||
CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{}));
|
||||
|
||||
m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
|
||||
m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
|
||||
|
||||
m_toRun = null;
|
||||
|
||||
for (Command command : m_commands.values()) {
|
||||
m_requirements.addAll(command.getRequirements());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new selectcommand.
|
||||
*
|
||||
* @param toRun a supplier providing the command to run
|
||||
*/
|
||||
public SelectCommand(Supplier<Command> toRun) {
|
||||
m_commands = null;
|
||||
m_selector = null;
|
||||
m_toRun = requireNonNullParam(toRun, "toRun", "SelectCommand");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
if (m_selector != null) {
|
||||
if (!m_commands.keySet().contains(m_selector.get())) {
|
||||
m_selectedCommand = new PrintCommand(
|
||||
"SelectCommand selector value does not correspond to" + " any command!");
|
||||
return;
|
||||
}
|
||||
m_selectedCommand = m_commands.get(m_selector.get());
|
||||
} else {
|
||||
m_selectedCommand = m_toRun.get();
|
||||
}
|
||||
m_selectedCommand.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_selectedCommand.execute();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_selectedCommand.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_selectedCommand.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
if (m_commands != null) {
|
||||
boolean runsWhenDisabled = true;
|
||||
for (Command command : m_commands.values()) {
|
||||
runsWhenDisabled &= command.runsWhenDisabled();
|
||||
}
|
||||
return runsWhenDisabled;
|
||||
} else {
|
||||
return m_toRun.get().runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class SequentialCommandGroup extends CommandGroupBase {
|
||||
private final List<Command> m_commands = new ArrayList<>();
|
||||
private int m_currentCommandIndex = -1;
|
||||
private boolean m_runWhenDisabled = true;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public SequentialCommandGroup(Command... commands) {
|
||||
addCommands(commands);
|
||||
}
|
||||
|
||||
@Override
|
||||
public final void addCommands(Command... commands) {
|
||||
requireUngrouped(commands);
|
||||
|
||||
if (m_currentCommandIndex != -1) {
|
||||
throw new IllegalStateException(
|
||||
"Commands cannot be added to a CommandGroup while the group is running");
|
||||
}
|
||||
|
||||
registerGroupedCommands(commands);
|
||||
|
||||
for (Command command : commands) {
|
||||
m_commands.add(command);
|
||||
m_requirements.addAll(command.getRequirements());
|
||||
m_runWhenDisabled &= command.runsWhenDisabled();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_currentCommandIndex = 0;
|
||||
|
||||
if (!m_commands.isEmpty()) {
|
||||
m_commands.get(0).initialize();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
if (m_commands.isEmpty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
Command currentCommand = m_commands.get(m_currentCommandIndex);
|
||||
|
||||
currentCommand.execute();
|
||||
if (currentCommand.isFinished()) {
|
||||
currentCommand.end(false);
|
||||
m_currentCommandIndex++;
|
||||
if (m_currentCommandIndex < m_commands.size()) {
|
||||
m_commands.get(m_currentCommandIndex).initialize();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1
|
||||
&& m_currentCommandIndex < m_commands.size()
|
||||
) {
|
||||
m_commands.get(m_currentCommandIndex).end(true);
|
||||
}
|
||||
m_currentCommandIndex = -1;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_currentCommandIndex == m_commands.size();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_runWhenDisabled;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* 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 {@link Command#withTimeout(double)} or
|
||||
* {@link Command#withInterrupt(BooleanSupplier)} to give it one.
|
||||
*/
|
||||
public class StartEndCommand extends CommandBase {
|
||||
protected final Runnable m_onInit;
|
||||
protected final Runnable m_onEnd;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) {
|
||||
m_onInit = requireNonNullParam(onInit, "onInit", "StartEndCommand");
|
||||
m_onEnd = requireNonNullParam(onEnd, "onEnd", "StartEndCommand");
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_onInit.run();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_onEnd.run();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.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 {@link Command}s. Subsystems are used by the
|
||||
* {@link 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 {@link Command#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
|
||||
* {@link CommandScheduler#registerSubsystem(Subsystem...)} method in order for the
|
||||
* {@link Subsystem#periodic()} method to be called. It is recommended that this method be called
|
||||
* from the constructor of users' Subsystem implementations. The {@link SubsystemBase}
|
||||
* class offers a simple base for user implementations that handles this.
|
||||
*/
|
||||
public interface Subsystem {
|
||||
|
||||
/**
|
||||
* This method is called periodically by the {@link CommandScheduler}. Useful for updating
|
||||
* subsystem-specific state that you don't want to offload to a {@link 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.
|
||||
*/
|
||||
default void periodic() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the default {@link 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 {@link Command#isFinished()}
|
||||
* method should always return false. Will automatically register this subsystem with the
|
||||
* {@link CommandScheduler}.
|
||||
*
|
||||
* @param defaultCommand the default command to associate with this subsystem
|
||||
*/
|
||||
default void setDefaultCommand(Command defaultCommand) {
|
||||
CommandScheduler.getInstance().setDefaultCommand(this, 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
|
||||
*/
|
||||
default Command getDefaultCommand() {
|
||||
return CommandScheduler.getInstance().getDefaultCommand(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
default Command getCurrentCommand() {
|
||||
return CommandScheduler.getInstance().requiring(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Registers this subsystem with the {@link CommandScheduler}, allowing its
|
||||
* {@link Subsystem#periodic()} method to be called when the scheduler runs.
|
||||
*/
|
||||
default void register() {
|
||||
CommandScheduler.getInstance().registerSubsystem(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import edu.wpi.first.wpilibj.Sendable;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
|
||||
|
||||
/**
|
||||
* A base for subsystems that handles registration in the constructor, and provides a more intuitive
|
||||
* method for setting the default command.
|
||||
*/
|
||||
public abstract class SubsystemBase implements Subsystem, Sendable {
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*/
|
||||
public SubsystemBase() {
|
||||
String name = this.getClass().getSimpleName();
|
||||
name = name.substring(name.lastIndexOf('.') + 1);
|
||||
SendableRegistry.addLW(this, name, name);
|
||||
CommandScheduler.getInstance().registerSubsystem(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the name of this Subsystem.
|
||||
*
|
||||
* @return Name
|
||||
*/
|
||||
@Override
|
||||
public String getName() {
|
||||
return SendableRegistry.getName(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the name of this Subsystem.
|
||||
*
|
||||
* @param name name
|
||||
*/
|
||||
@Override
|
||||
public void setName(String name) {
|
||||
SendableRegistry.setName(this, name);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the subsystem name of this Subsystem.
|
||||
*
|
||||
* @return Subsystem name
|
||||
*/
|
||||
@Override
|
||||
public String getSubsystem() {
|
||||
return SendableRegistry.getSubsystem(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the subsystem name of this Subsystem.
|
||||
*
|
||||
* @param subsystem subsystem name
|
||||
*/
|
||||
@Override
|
||||
public void setSubsystem(String subsystem) {
|
||||
SendableRegistry.setSubsystem(this, subsystem);
|
||||
}
|
||||
|
||||
/**
|
||||
* Associates a {@link Sendable} with this Subsystem.
|
||||
* Also update the child's name.
|
||||
*
|
||||
* @param name name to give child
|
||||
* @param child sendable
|
||||
*/
|
||||
public void addChild(String name, Sendable child) {
|
||||
SendableRegistry.addLW(child, getSubsystem(), name);
|
||||
SendableRegistry.addChild(this, child);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Subsystem");
|
||||
|
||||
builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
|
||||
builder.addStringProperty(".default",
|
||||
() -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", null);
|
||||
builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
|
||||
builder.addStringProperty(".command",
|
||||
() -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none", null);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import java.util.function.Consumer;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
|
||||
|
||||
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism
|
||||
* motion.
|
||||
*/
|
||||
public class TrapezoidProfileCommand extends CommandBase {
|
||||
private final TrapezoidProfile m_profile;
|
||||
private final Consumer<State> m_output;
|
||||
|
||||
private final Timer m_timer = new Timer();
|
||||
|
||||
/**
|
||||
* Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
|
||||
* Output will be piped to the provided consumer function.
|
||||
*
|
||||
* @param profile The motion profile to execute.
|
||||
* @param output The consumer for the profile output.
|
||||
* @param requirements The subsystems required by this command.
|
||||
*/
|
||||
public TrapezoidProfileCommand(TrapezoidProfile profile,
|
||||
Consumer<State> output,
|
||||
Subsystem... requirements) {
|
||||
m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
|
||||
m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_timer.reset();
|
||||
m_timer.start();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_output.accept(m_profile.calculate(m_timer.get()));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_timer.stop();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_timer.hasPeriodPassed(m_profile.totalTime());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public class WaitCommand extends CommandBase {
|
||||
protected Timer m_timer = new Timer();
|
||||
private final double m_duration;
|
||||
|
||||
/**
|
||||
* Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
|
||||
*
|
||||
* @param seconds the time to wait, in seconds
|
||||
*/
|
||||
public WaitCommand(double seconds) {
|
||||
m_duration = seconds;
|
||||
SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_timer.reset();
|
||||
m_timer.start();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_timer.stop();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_timer.hasPeriodPassed(m_duration);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A command that does nothing but ends after a specified match time or condition. Useful for
|
||||
* CommandGroups.
|
||||
*/
|
||||
public class WaitUntilCommand extends CommandBase {
|
||||
private final BooleanSupplier m_condition;
|
||||
|
||||
/**
|
||||
* Creates a new WaitUntilCommand that ends after a given condition becomes true.
|
||||
*
|
||||
* @param condition the condition to determine when to end
|
||||
*/
|
||||
public WaitUntilCommand(BooleanSupplier condition) {
|
||||
m_condition = requireNonNullParam(condition, "condition", "WaitUntilCommand");
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public WaitUntilCommand(double time) {
|
||||
this(() -> Timer.getMatchTime() - time > 0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_condition.getAsBoolean();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,211 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command.button;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
/**
|
||||
* This class provides an easy way to link commands to OI inputs.
|
||||
*
|
||||
* <p>It is very easy to link a button to a command. For instance, you could link the trigger
|
||||
* button of a joystick to a "score" command.
|
||||
*
|
||||
* <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
|
||||
* operator interface as a common use case of the more generalized Trigger objects. This is a simple
|
||||
* wrapper around Trigger with the method names renamed to fit the Button object use.
|
||||
*/
|
||||
@SuppressWarnings("PMD.TooManyMethods")
|
||||
public abstract class Button extends Trigger {
|
||||
/**
|
||||
* Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
|
||||
* overridden).
|
||||
*/
|
||||
public Button() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new button with the given condition determining whether it is pressed.
|
||||
*
|
||||
* @param isPressed returns whether or not the trigger should be active
|
||||
*/
|
||||
public Button(BooleanSupplier isPressed) {
|
||||
super(isPressed);
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command whenever the button is newly pressed.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenPressed(final Command command, boolean interruptible) {
|
||||
whenActive(command, interruptible);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command whenever the button is newly pressed. The command is set to be
|
||||
* interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenPressed(final Command command) {
|
||||
whenActive(command);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the given runnable whenever the button is newly pressed.
|
||||
*
|
||||
* @param toRun the runnable to run
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenPressed(final Runnable toRun) {
|
||||
whenActive(toRun);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constantly starts the given command while the button is held.
|
||||
*
|
||||
* {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
|
||||
* be canceled when the button is released.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whileHeld(final Command command, boolean interruptible) {
|
||||
whileActiveContinuous(command, interruptible);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constantly starts the given command while the button is held.
|
||||
*
|
||||
* {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
|
||||
* be canceled when the button is released. The command is set to be interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whileHeld(final Command command) {
|
||||
whileActiveContinuous(command);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constantly runs the given runnable while the button is held.
|
||||
*
|
||||
* @param toRun the runnable to run
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whileHeld(final Runnable toRun) {
|
||||
whileActiveContinuous(toRun);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command when the button is first pressed, and cancels it when it is released,
|
||||
* but does not start it again if it ends or is otherwise interrupted.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenHeld(final Command command, boolean interruptible) {
|
||||
whileActiveOnce(command, interruptible);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command when the button is first pressed, and cancels it when it is released,
|
||||
* but does not start it again if it ends or is otherwise interrupted. The command is set to be
|
||||
* interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenHeld(final Command command) {
|
||||
whileActiveOnce(command, true);
|
||||
return this;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Starts the command when the button is released.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenReleased(final Command command, boolean interruptible) {
|
||||
whenInactive(command, interruptible);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the command when the button is released. The command is set to be interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenReleased(final Command command) {
|
||||
whenInactive(command);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the given runnable when the button is released.
|
||||
*
|
||||
* @param toRun the runnable to run
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button whenReleased(final Runnable toRun) {
|
||||
whenInactive(toRun);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggles the command whenever the button is pressed (on then off then on).
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
*/
|
||||
public Button toggleWhenPressed(final Command command, boolean interruptible) {
|
||||
toggleWhenActive(command, interruptible);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggles the command whenever the button is pressed (on then off then on). The command is set
|
||||
* to be interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button toggleWhenPressed(final Command command) {
|
||||
toggleWhenActive(command);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Cancels the command when the button is pressed.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this button, so calls can be chained
|
||||
*/
|
||||
public Button cancelWhenPressed(final Command command) {
|
||||
cancelWhenActive(command);
|
||||
return this;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command.button;
|
||||
|
||||
/**
|
||||
* This class is intended to be used within a program. The programmer can manually set its value.
|
||||
* Also includes a setting for whether or not it should invert its value.
|
||||
*/
|
||||
public class InternalButton extends Button {
|
||||
private boolean m_pressed;
|
||||
private boolean m_inverted;
|
||||
|
||||
/**
|
||||
* Creates an InternalButton that is not inverted.
|
||||
*/
|
||||
public InternalButton() {
|
||||
this(false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an InternalButton which is inverted depending on the input.
|
||||
*
|
||||
* @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
|
||||
* when set to false.
|
||||
*/
|
||||
public InternalButton(boolean inverted) {
|
||||
m_pressed = m_inverted = inverted;
|
||||
}
|
||||
|
||||
public void setInverted(boolean inverted) {
|
||||
m_inverted = inverted;
|
||||
}
|
||||
|
||||
public void setPressed(boolean pressed) {
|
||||
m_pressed = pressed;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean get() {
|
||||
return m_pressed ^ m_inverted;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command.button;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A {@link Button} that gets its state from a {@link GenericHID}.
|
||||
*/
|
||||
public class JoystickButton extends Button {
|
||||
private final GenericHID m_joystick;
|
||||
private final int m_buttonNumber;
|
||||
|
||||
/**
|
||||
* Creates a joystick button for triggering commands.
|
||||
*
|
||||
* @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick,
|
||||
* etc)
|
||||
* @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
|
||||
*/
|
||||
public JoystickButton(GenericHID joystick, int buttonNumber) {
|
||||
requireNonNullParam(joystick, "joystick", "JoystickButton");
|
||||
|
||||
m_joystick = joystick;
|
||||
m_buttonNumber = buttonNumber;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the value of the joystick button.
|
||||
*
|
||||
* @return The value of the joystick button
|
||||
*/
|
||||
@Override
|
||||
public boolean get() {
|
||||
return m_joystick.getRawButton(m_buttonNumber);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command.button;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* A {@link Button} that gets its state from a POV on a {@link GenericHID}.
|
||||
*/
|
||||
public class POVButton extends Button {
|
||||
private final GenericHID m_joystick;
|
||||
private final int m_angle;
|
||||
private final int m_povNumber;
|
||||
|
||||
/**
|
||||
* Creates a POV button for triggering commands.
|
||||
*
|
||||
* @param joystick The GenericHID object that has the POV
|
||||
* @param angle The desired angle in degrees (e.g. 90, 270)
|
||||
* @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
|
||||
*/
|
||||
public POVButton(GenericHID joystick, int angle, int povNumber) {
|
||||
requireNonNullParam(joystick, "joystick", "POVButton");
|
||||
|
||||
m_joystick = joystick;
|
||||
m_angle = angle;
|
||||
m_povNumber = povNumber;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a POV button for triggering commands.
|
||||
* By default, acts on POV 0
|
||||
*
|
||||
* @param joystick The GenericHID object that has the POV
|
||||
* @param angle The desired angle (e.g. 90, 270)
|
||||
*/
|
||||
public POVButton(GenericHID joystick, int angle) {
|
||||
this(joystick, angle, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks whether the current value of the POV is the target angle.
|
||||
*
|
||||
* @return Whether the value of the POV matches the target angle
|
||||
*/
|
||||
@Override
|
||||
public boolean get() {
|
||||
return m_joystick.getPOV(m_povNumber) == m_angle;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,351 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj2.command.button;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
/**
|
||||
* This class provides an easy way to link commands to inputs.
|
||||
*
|
||||
* <p>It is very easy to link a button to a command. For instance, you could link the trigger
|
||||
* button of a joystick to a "score" command.
|
||||
*
|
||||
* <p>It is encouraged that teams write a subclass of Trigger if they want to have something
|
||||
* unusual (for instance, if they want to react to the user holding a button while the robot is
|
||||
* reading a certain sensor input). For this, they only have to write the {@link Trigger#get()}
|
||||
* method to get the full functionality of the Trigger class.
|
||||
*/
|
||||
@SuppressWarnings("PMD.TooManyMethods")
|
||||
public class Trigger {
|
||||
private final BooleanSupplier m_isActive;
|
||||
|
||||
/**
|
||||
* Creates a new trigger with the given condition determining whether it is active.
|
||||
*
|
||||
* @param isActive returns whether or not the trigger should be active
|
||||
*/
|
||||
public Trigger(BooleanSupplier isActive) {
|
||||
m_isActive = isActive;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new trigger that is always inactive. Useful only as a no-arg constructor for
|
||||
* subclasses that will be overriding {@link Trigger#get()} anyway.
|
||||
*/
|
||||
public Trigger() {
|
||||
m_isActive = () -> false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether or not the trigger is active.
|
||||
*
|
||||
* <p>This method will be called repeatedly a command is linked to the Trigger.
|
||||
*
|
||||
* @return whether or not the trigger condition is active.
|
||||
*/
|
||||
public boolean get() {
|
||||
return m_isActive.getAsBoolean();
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command whenever the trigger just becomes active.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whenActive(final Command command, boolean interruptible) {
|
||||
requireNonNullParam(command, "command", "whenActive");
|
||||
|
||||
CommandScheduler.getInstance().addButton(new Runnable() {
|
||||
private boolean m_pressedLast = get();
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
boolean pressed = get();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
command.schedule(interruptible);
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
});
|
||||
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command whenever the trigger just becomes active. The command is set to be
|
||||
* interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whenActive(final Command command) {
|
||||
return whenActive(command, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the given runnable whenever the trigger just becomes active.
|
||||
*
|
||||
* @param toRun the runnable to run
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whenActive(final Runnable toRun) {
|
||||
return whenActive(new InstantCommand(toRun));
|
||||
}
|
||||
|
||||
/**
|
||||
* Constantly starts the given command while the button is held.
|
||||
*
|
||||
* {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
|
||||
* will be canceled when the trigger becomes inactive.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
|
||||
requireNonNullParam(command, "command", "whileActiveContinuous");
|
||||
|
||||
CommandScheduler.getInstance().addButton(new Runnable() {
|
||||
private boolean m_pressedLast = get();
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
boolean pressed = get();
|
||||
|
||||
if (pressed) {
|
||||
command.schedule(interruptible);
|
||||
} else if (m_pressedLast) {
|
||||
command.cancel();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
});
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constantly starts the given command while the button is held.
|
||||
*
|
||||
* {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
|
||||
* will be canceled when the trigger becomes inactive. The command is set to be interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whileActiveContinuous(final Command command) {
|
||||
return whileActiveContinuous(command, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constantly runs the given runnable while the button is held.
|
||||
*
|
||||
* @param toRun the runnable to run
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whileActiveContinuous(final Runnable toRun) {
|
||||
return whileActiveContinuous(new InstantCommand(toRun));
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command when the trigger initially becomes active, and ends it when it becomes
|
||||
* inactive, but does not re-start it in-between.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whileActiveOnce(final Command command, boolean interruptible) {
|
||||
requireNonNullParam(command, "command", "whileActiveOnce");
|
||||
|
||||
CommandScheduler.getInstance().addButton(new Runnable() {
|
||||
private boolean m_pressedLast = get();
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
boolean pressed = get();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
command.schedule(interruptible);
|
||||
} else if (m_pressedLast && !pressed) {
|
||||
command.cancel();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
});
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the given command when the trigger initially becomes active, and ends it when it becomes
|
||||
* inactive, but does not re-start it in-between. The command is set to be interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whileActiveOnce(final Command command) {
|
||||
return whileActiveOnce(command, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the command when the trigger becomes inactive.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whenInactive(final Command command, boolean interruptible) {
|
||||
requireNonNullParam(command, "command", "whenInactive");
|
||||
|
||||
CommandScheduler.getInstance().addButton(new Runnable() {
|
||||
private boolean m_pressedLast = get();
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
boolean pressed = get();
|
||||
|
||||
if (m_pressedLast && !pressed) {
|
||||
command.schedule(interruptible);
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
});
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Starts the command when the trigger becomes inactive. The command is set to be interruptible.
|
||||
*
|
||||
* @param command the command to start
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whenInactive(final Command command) {
|
||||
return whenInactive(command, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the given runnable when the trigger becomes inactive.
|
||||
*
|
||||
* @param toRun the runnable to run
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger whenInactive(final Runnable toRun) {
|
||||
return whenInactive(new InstantCommand(toRun));
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggles a command when the trigger becomes active.
|
||||
*
|
||||
* @param command the command to toggle
|
||||
* @param interruptible whether the command is interruptible
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger toggleWhenActive(final Command command, boolean interruptible) {
|
||||
requireNonNullParam(command, "command", "toggleWhenActive");
|
||||
|
||||
CommandScheduler.getInstance().addButton(new Runnable() {
|
||||
private boolean m_pressedLast = get();
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
boolean pressed = get();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
if (command.isScheduled()) {
|
||||
command.cancel();
|
||||
} else {
|
||||
command.schedule(interruptible);
|
||||
}
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
});
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Toggles a command when the trigger becomes active. The command is set to be interruptible.
|
||||
*
|
||||
* @param command the command to toggle
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger toggleWhenActive(final Command command) {
|
||||
return toggleWhenActive(command, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Cancels a command when the trigger becomes active.
|
||||
*
|
||||
* @param command the command to cancel
|
||||
* @return this trigger, so calls can be chained
|
||||
*/
|
||||
public Trigger cancelWhenActive(final Command command) {
|
||||
requireNonNullParam(command, "command", "cancelWhenActive");
|
||||
|
||||
CommandScheduler.getInstance().addButton(new Runnable() {
|
||||
private boolean m_pressedLast = get();
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
boolean pressed = get();
|
||||
|
||||
if (!m_pressedLast && pressed) {
|
||||
command.cancel();
|
||||
}
|
||||
|
||||
m_pressedLast = pressed;
|
||||
}
|
||||
});
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Composes this trigger with another trigger, returning a new trigger that is active when both
|
||||
* triggers are active.
|
||||
*
|
||||
* @param trigger the trigger to compose with
|
||||
* @return the trigger that is active when both triggers are active
|
||||
*/
|
||||
public Trigger and(Trigger trigger) {
|
||||
return new Trigger(() -> get() && trigger.get());
|
||||
}
|
||||
|
||||
/**
|
||||
* Composes this trigger with another trigger, returning a new trigger that is active when either
|
||||
* trigger is active.
|
||||
*
|
||||
* @param trigger the trigger to compose with
|
||||
* @return the trigger that is active when either trigger is active
|
||||
*/
|
||||
public Trigger or(Trigger trigger) {
|
||||
return new Trigger(() -> get() || trigger.get());
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
|
||||
* negation of this trigger.
|
||||
*
|
||||
* @return the negated trigger
|
||||
*/
|
||||
public Trigger negate() {
|
||||
return new Trigger(() -> !get());
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user